Fixed small bug with forward kinematics

trajectory-invariants · Jun 28, 2024 · b33dd38 · b33dd38
1 parent 1ae5426
commit b33dd38
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 3 deletions.
diff --git a/invariants_py/kinematics/robot_forward_kinematics.py b/invariants_py/kinematics/robot_forward_kinematics.py
@@ -9,13 +9,12 @@
 from invariants_py.kinematics.rigidbody_kinematics import rotate_z
 from math import pi
 
-def robot_forward_kinematics(q, path_to_urdf, root = 'base_link', tip = 'tool0'):
+def robot_forward_kinematics(q, path_to_urdf, root = 'world', tip = 'tool0'):
     ur10 = u2c.URDFparser()
     ur10.from_file(path_to_urdf)
     fk_dict = ur10.get_forward_kinematics(root, tip)
     robot_forward_kinematics = fk_dict["T_fk"]
     T_rob = robot_forward_kinematics(q.T)
-    T_rob = rotate_z(pi) @ T_rob
     p_obj = T_rob[:3,3]
     R_obj = T_rob[:3,:3]
 

diff --git a/invariants_py/kinematics/robot_inverse_kinematics_rockit.py b/invariants_py/kinematics/robot_inverse_kinematics_rockit.py
@@ -87,7 +87,7 @@ def inv_kin(q_init, q_joint_lim, des_p_obj, des_R_obj, path_to_urdf, root = 'bas
     trajectory,time = dh.read_pose_trajectory_from_txt(data_location)
     pose,time_profile,arclength,nb_samples,stepsize = reparam.reparameterize_trajectory_arclength(trajectory)
     N = 100
-    root_link_name = "base_link"
+    root_link_name = "world"
     tip_link_name = "TCP_frame"
     path_to_urdf = dh.find_data_path('robot/ur10.urdf')
     startpos = [0.3056, 0.0635, 0.441]