@@ -52,23 +52,23 @@ def __init__(
5252 # Initialize frame task for IK
5353 self .tip_frame = self .solver .add_frame_task (self .target_frame_name , np .eye (4 ))
5454
55- def forward_kinematics (self , robot_pos_deg ):
55+ def forward_kinematics (self , joint_pos_deg ):
5656 """
5757 Compute forward kinematics for given joint configuration given the target frame name in the constructor.
5858
5959 Args:
60- robot_pos_deg : Joint positions in degrees (numpy array)
60+ joint_pos_deg : Joint positions in degrees (numpy array)
6161
6262 Returns:
6363 4x4 transformation matrix of the end-effector pose
6464 """
6565
6666 # Convert degrees to radians
67- robot_pos_rad = np .deg2rad (robot_pos_deg [: len (self .joint_names )])
67+ joint_pos_rad = np .deg2rad (joint_pos_deg [: len (self .joint_names )])
6868
6969 # Update joint positions in placo robot
7070 for i , joint_name in enumerate (self .joint_names ):
71- self .robot .set_joint (joint_name , robot_pos_rad [i ])
71+ self .robot .set_joint (joint_name , joint_pos_rad [i ])
7272
7373 # Update kinematics
7474 self .robot .update_kinematics ()
@@ -110,19 +110,19 @@ def inverse_kinematics(
110110 self .robot .update_kinematics ()
111111
112112 # Extract joint positions
113- joint_positions_rad = []
113+ joint_pos_rad = []
114114 for joint_name in self .joint_names :
115115 joint = self .robot .get_joint (joint_name )
116- joint_positions_rad .append (joint )
116+ joint_pos_rad .append (joint )
117117
118118 # Convert back to degrees
119- joint_positions_deg = np .rad2deg (joint_positions_rad )
119+ joint_pos_deg = np .rad2deg (joint_pos_rad )
120120
121121 # Preserve gripper position if present in current_joint_pos
122122 if len (current_joint_pos ) > len (self .joint_names ):
123123 result = np .zeros_like (current_joint_pos )
124- result [: len (self .joint_names )] = joint_positions_deg
124+ result [: len (self .joint_names )] = joint_pos_deg
125125 result [len (self .joint_names ) :] = current_joint_pos [len (self .joint_names ) :]
126126 return result
127127 else :
128- return joint_positions_deg
128+ return joint_pos_deg
0 commit comments