Minor changes

invariants_py/generate_trajectory/rockit_generate_pose_traj_from_vector_invars.py

@@ -29,7 +29,7 @@ def __init__(self, boundary_constraints, window_len = 100, fatrop_solver = False
         if include_robot_model:
             robot = urdf.URDF.load(path_to_urdf)
             nb_joints = robot_params.get('joint_number', robot.num_actuated_joints)
-            q_limits = robot_params.get('q_lim', [robot._actuated_joints[i].limit.upper for i in range(robot.num_actuated_joints)])
+            q_limits = robot_params.get('q_lim', np.array([robot._actuated_joints[i].limit.upper for i in range(robot.num_actuated_joints)]))
             root = robot_params.get('root', robot.base_link)
             tip = robot_params.get('tip', 'tool0')
             q_init = robot_params.get('q_init', np.zeros(nb_joints))
@@ -245,20 +245,21 @@ def __init__(self, boundary_constraints, window_len = 100, fatrop_solver = False
             ocp.sample(R_t, grid='control')[1], # sampled translational FS frame
             ocp.sample(R_r, grid='control')[1], # sampled rotational FS frame
             ocp.sample(R_obj, grid='control')[1]] # sampled object orientation
+        input_params_labels = ["invars","w_invars","stepsize"] # input labels for debugging
+        input_sol_labels = ["invars1","p_obj1","R_t1","R_r1","R_obj1"] # labels for debugging
+        solution_labels = ["invars2","p_obj2","R_t2","R_r2","R_obj2"] # output labels for debugging
         if include_robot_model:
             solution.append(ocp.sample(q, grid='control')[1]) # sampled joint values
-            inputs_labels = ["invars","w_invars","stepsize","q_lim","invars1","p_obj1","R_t1","R_r1","R_obj1","q1"] # input labels for debugging
-            solution_labels = ["invars2","p_obj2","R_t2","R_r2","R_obj2","q2"] # output labels for debugging
-        else:
-            inputs_labels = ["invars","w_invars","stepsize","invars1","p_obj1","R_t1","R_r1","R_obj1"] # input labels for debugging
-            solution_labels = ["invars2","p_obj2","R_t2","R_r2","R_obj2"] # output labels for debugging
+            input_params_labels.append("q_lim")
+            input_sol_labels.append("q1")
+            solution_labels.append("q2")
 
         self.ocp = ocp # save the optimization problem locally, avoids problems when multiple rockit ocp's are created
 
         self.ocp_function = self.ocp.to_function('ocp_function', 
             [*input_params,*solution,*bounds], # inputs
             [*solution], # outputs
-            [*inputs_labels,*bounds_labels], # input labels for debugging
+            [*input_params_labels,*input_sol_labels,*bounds_labels], # input labels for debugging
             [*solution_labels], # output labels for debugging
         )
 
@@ -300,7 +301,8 @@ def __init__(self, boundary_constraints, window_len = 100, fatrop_solver = False
             self.q = q
             self.qdot = qdot
             self.q_init = q_init
-            self.q_lim = q_limits
+            self.q_lim = q_lim
+            self.q_limits = q_limits
 
     def generate_trajectory(self, invariant_model, boundary_constraints, step_size, weights_params = {}, initial_values = {}):
 
@@ -319,7 +321,7 @@ def generate_trajectory(self, invariant_model, boundary_constraints, step_size,
             w_invars[:, w_high_start:w_high_end+1] = w_high_invars.reshape(-1, 1)
 
         if self.include_robot_model:
-            input_params = [invariant_model.T,w_invars,step_size,self.q_lim]
+            input_params = [invariant_model.T,w_invars,step_size,self.q_limits]
         else:
             input_params = [invariant_model.T,w_invars,step_size]
 

invariants_py/kinematics/robot_forward_kinematics.py

@@ -18,4 +18,13 @@ def robot_forward_kinematics(q, path_to_urdf, root = 'world', tip = 'tool0'):
     p_obj = T_rob[:3,3]
     R_obj = T_rob[:3,:3]
 
-    return p_obj, R_obj
+    return p_obj, R_obj
+
+
+if __name__ == "__main__":
+    import numpy as np
+    q = np.array([-2.46888802,-0.42693144,-0.02180152, -0.433554,   -2.14098558,  1.77062242])
+    path_to_urdf = "/home/riccardo/code/invariants_py/invariants_py/data/robot/ur10.urdf"
+    tip = "TCP_frame"
+    pos, R = robot_forward_kinematics(q,path_to_urdf,tip=tip)
+    print(pos)

invariants_py/kinematics/robot_inverse_kinematics_rockit.py

@@ -78,8 +78,9 @@ def inv_kin(q_init, q_joint_lim, des_p_obj, des_R_obj, path_to_urdf, root = 'bas
 
     # joint_val = sol.value(q)
     _,joint_val = sol.sample(q,grid='control')
+    _, p_sol = sol.sample(p_obj,grid='control')
 
-    return joint_val
+    return joint_val, p_sol
 
 
 if __name__ == "__main__":
@@ -91,28 +92,32 @@ def inv_kin(q_init, q_joint_lim, des_p_obj, des_R_obj, path_to_urdf, root = 'bas
     tip_link_name = "TCP_frame"
     path_to_urdf = dh.find_data_path('robot/ur10.urdf')
     startpos = [0.3056, 0.0635, 0.441]
-    des_p_obj = pose[:,:3,3]  + startpos #[0.6818214 , 0.23448511, 0.39779707] * np.ones((N,3)) #
+    des_p_obj = [0.827,0.7144,0.552] * np.ones((N,3)) #pose[:,:3,3]  + startpos #[0.6818214 , 0.23448511, 0.39779707] * np.ones((N,3)) #
     des_R_obj = pose[:,:3,:3]
     # des_R_obj = np.zeros((N,3,3))
     # for i in range(N):
     #     des_R_obj[i] = pose[-1,:3,:3] #np.eye(3)
     q_init = [-pi, -2.27, 2.27, -pi/2, -pi/2, pi/4] * np.ones((N,6))
     q_joint_lim = [2*pi, 2*pi, pi, 2*pi, 2*pi, 2*pi]
-    q = inv_kin(q_init, q_joint_lim, des_p_obj, des_R_obj, path_to_urdf, root_link_name, tip_link_name, N)
+    q, p_sol = inv_kin(q_init, q_joint_lim, des_p_obj, des_R_obj, path_to_urdf, root_link_name, tip_link_name, N)
 
     print(q)
-
+    print(p_sol)
 # =======================================
 # Debugging =======================================
 # =======================================
-# p_obj, R_obj = robot_forward_kinematics(q,path_to_urdf,root_link_name,tip_link_name)
+    # p_obj, R_obj = robot_forward_kinematics(q[-1],path_to_urdf,root_link_name,tip_link_name)
 
-# for i in range(N):
-#     p_obj, R_obj = robot_forward_kinematics(q[i],path_to_urdf,root_link_name,tip_link_name)
+    # print(p_obj)
 
-#     #e_pos = cas.dot(p_obj - des_p_obj[i],p_obj - des_p_obj[i])
-#     e_rot = cas.dot(des_R_obj[i].T @ R_obj - np.eye(3),des_R_obj[i].T @ R_obj - np.eye(3))
-
-#     #print(des_R_obj[0])
-#     #print(R_obj)
-#     print(e_rot)
+    for i in range(N):
+        p_obj, R_obj = robot_forward_kinematics(q[i],path_to_urdf,root_link_name,tip_link_name)
+
+        e_pos = cas.dot(p_obj - des_p_obj[i],p_obj - des_p_obj[i])
+        print(e_pos)
+        # e_rot = cas.dot(des_R_obj[i].T @ R_obj - np.eye(3),des_R_obj[i].T @ R_obj - np.eye(3))
+
+        #print(des_R_obj[0])
+        #print(R_obj)
+        # print(e_rot)
+    print(p_obj)