From afcdc3f63e9940f6c4db5397a51deebc26ad59af Mon Sep 17 00:00:00 2001 From: Masaki Murooka Date: Thu, 19 Dec 2024 16:19:55 +0900 Subject: [PATCH] Refactor: Add forward_kinematics method. --- robo_manip_baselines/common/MotionManager.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/robo_manip_baselines/common/MotionManager.py b/robo_manip_baselines/common/MotionManager.py index 56662c4a..23da2879 100644 --- a/robo_manip_baselines/common/MotionManager.py +++ b/robo_manip_baselines/common/MotionManager.py @@ -29,7 +29,7 @@ def __init__(self, env): self.joint_pos = self.env.unwrapped.init_qpos[ self.env.unwrapped.ik_arm_joint_ids ].copy() - pin.forwardKinematics(self.pin_model, self.pin_data, self.joint_pos) + self.forward_kinematics() self._original_target_se3 = self.pin_data.oMi[ self.env.unwrapped.ik_eef_joint_id ].copy() @@ -50,6 +50,9 @@ def reset(self): self.env.unwrapped.gripper_action_idx ] + def forward_kinematics(self): + pin.forwardKinematics(self.pin_model, self.pin_data, self.joint_pos) + def inverse_kinematics(self): """Solve inverse kinematics.""" # https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/md_doc_b-examples_d-inverse-kinematics.html @@ -71,7 +74,7 @@ def inverse_kinematics(self): ) ) self.joint_pos = pin.integrate(self.pin_model, self.joint_pos, delta_joint_pos) - pin.forwardKinematics(self.pin_model, self.pin_data, self.joint_pos) + self.forward_kinematics() def set_relative_target_se3(self, delta_pos=None, delta_rpy=None): """Set the target pose of the end-effector relatively."""