diff --git a/invariants_py/generate_trajectory/rockit_generate_pose_traj_from_vector_invars.py b/invariants_py/generate_trajectory/rockit_generate_pose_traj_from_vector_invars.py index eef82f6..8697d07 100644 --- a/invariants_py/generate_trajectory/rockit_generate_pose_traj_from_vector_invars.py +++ b/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] diff --git a/invariants_py/kinematics/robot_forward_kinematics.py b/invariants_py/kinematics/robot_forward_kinematics.py index 89bf009..3aceecb 100644 --- a/invariants_py/kinematics/robot_forward_kinematics.py +++ b/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 \ No newline at end of file + 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) \ No newline at end of file diff --git a/invariants_py/kinematics/robot_inverse_kinematics_rockit.py b/invariants_py/kinematics/robot_inverse_kinematics_rockit.py index c1c1975..add59e4 100644 --- a/invariants_py/kinematics/robot_inverse_kinematics_rockit.py +++ b/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) \ No newline at end of file + 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) \ No newline at end of file