Skip to content

Commit cd0a5d6

Browse files
Unifiying names in kinematics.py for consistency
1 parent 4c144a7 commit cd0a5d6

File tree

1 file changed

+9
-9
lines changed

1 file changed

+9
-9
lines changed

src/lerobot/model/kinematics.py

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -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

Comments
 (0)