Refactor: Add forward_kinematics method.

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."""