Removed orientation weight from configuration and added it as an argument in inverse kinematics

Co-authored-by: CarolinePascal <[email protected]>

Author: michel-aractingi

lerobot/common/model/kinematics.py

@@ -23,7 +23,6 @@ def __init__(
         urdf_path: str,
         target_frame_name: str = "gripperframe",
         joint_names: list[str] = None,
-        orientation_weight: float = 0.01,
     ):
         """
         Initialize placo-based kinematics solver.
@@ -32,7 +31,6 @@ def __init__(
             urdf_path: Path to the robot URDF file
             target_frame_name: Name of the end-effector frame in the URDF
             joint_names: List of joint names to use for the kinematics solver
-            orientation_weight: Weight for orientation constraint in IK, set to 0.0 to only constrain position
         """
         try:
             import placo
@@ -54,9 +52,6 @@ def __init__(
         # Initialize frame task for IK
         self.tip_frame = self.solver.add_frame_task(self.target_frame_name, np.eye(4))
 
-        # Set orientation weight
-        self.orientation_weight = orientation_weight
-
     def forward_kinematics(self, robot_pos_deg):
         """
         Compute forward kinematics for given joint configuration given the target frame name in the constructor.
@@ -81,13 +76,15 @@ def forward_kinematics(self, robot_pos_deg):
         # Get the transformation matrix
         return self.robot.get_T_world_frame(self.target_frame_name)
 
-    def inverse_kinematics(self, current_joint_pos, desired_ee_pose):
+    def inverse_kinematics(self, current_joint_pos, desired_ee_pose, position_weight=1.0, orientation_weight=0.01):
         """
         Compute inverse kinematics using placo solver.
 
         Args:
             current_joint_pos: Current joint positions in degrees (used as initial guess)
             desired_ee_pose: Target end-effector pose as a 4x4 transformation matrix
+            position_weight: Weight for position constraint in IK
+            orientation_weight: Weight for orientation constraint in IK, set to 0.0 to only constrain position
 
         Returns:
             Joint positions in degrees that achieve the desired end-effector pose
@@ -104,7 +101,7 @@ def inverse_kinematics(self, current_joint_pos, desired_ee_pose):
         self.tip_frame.T_world_frame = desired_ee_pose
 
         # Configure the task based on position_only flag
-        self.tip_frame.configure(self.target_frame_name, "soft", 1.0, self.orientation_weight)
+        self.tip_frame.configure(self.target_frame_name, "soft", position_weight, orientation_weight)
 
         # Solve IK
         self.solver.solve(True)

lerobot/common/robots/so100_follower/config_so100_follower.py

@@ -50,9 +50,6 @@ class SO100FollowerEndEffectorConfig(SO100FollowerConfig):
     # End-effector frame name in the URDF
     target_frame_name: str = "gripperframe"
 
-    # Weight for orientation constraint in IK
-    orientation_weight: float = 0.01
-
     # Default bounds for the end-effector position (in meters)
     end_effector_bounds: dict[str, list[float]] = field(
         default_factory=lambda: {

lerobot/common/robots/so100_follower/so100_follower_end_effector.py

@@ -72,7 +72,6 @@ def __init__(self, config: SO100FollowerEndEffectorConfig):
         self.kinematics = RobotKinematics(
             urdf_path=self.config.urdf_path,
             target_frame_name=self.config.target_frame_name,
-            orientation_weight=self.config.orientation_weight,
         )
 
         # Store the bounds for end-effector position