Skip to content

Commit a3fb53f

Browse files
authored
Add full naming convention aliases for robot limbs (#641)
* Add full naming convention aliases for robot limbs Add right_arm, left_arm, right_leg, left_leg properties to RobotModel base class with isinstance type checking and NotImplementedError handling. Also add backward-compatible aliases to individual robot models: - PR2: right_arm, left_arm, right_arm_with_torso, left_arm_with_torso - Panda, Fetch, Kuka: arm, arm_with_torso (for single-arm robots) - Nextage, R8_6: right_arm, left_arm This allows using full names (robot.right_arm) while maintaining backward compatibility with abbreviated names (robot.rarm). * Add test for limb naming convention aliases Test right_arm, left_arm, arm aliases and their end_coords variants for dual-arm robots (PR2) and single-arm robots (Fetch, Kuka). Also verify that non-existent limbs (legs) return None. * Update examples to use new limb naming convention - batch_ik_demo.py: Use arm/right_arm instead of rarm - pybullet_robot_interface.py: Use arm/arm_end_coords instead of rarm * Update examples to use new naming convention (right_arm, left_arm) * Address review: use @Property wrappers instead of direct assignment
1 parent 3f457d6 commit a3fb53f

File tree

15 files changed

+284
-51
lines changed

15 files changed

+284
-51
lines changed

examples/batch_ik_demo.py

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -91,19 +91,19 @@ def main():
9191
# Initialize robot based on selection
9292
if args.robot == 'fetch':
9393
robot = Fetch()
94-
arm = robot.rarm_with_torso if args.with_torso else robot.rarm
94+
arm = robot.arm_with_torso if args.with_torso else robot.arm
9595
elif args.robot == 'pr2':
9696
robot = PR2()
97-
arm = robot.rarm_with_torso if args.with_torso else robot.rarm
97+
arm = robot.right_arm_with_torso if args.with_torso else robot.right_arm
9898
elif args.robot == 'panda':
9999
robot = Panda()
100-
arm = robot.rarm
100+
arm = robot.arm
101101
elif args.robot == 'r8_6':
102102
robot = R8_6()
103-
arm = robot.rarm
103+
arm = robot.right_arm
104104
elif args.robot == 'nextage':
105105
robot = Nextage()
106-
arm = robot.rarm
106+
arm = robot.right_arm
107107

108108
robot.reset_pose()
109109

examples/collision_free_trajectory.py

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -79,13 +79,13 @@
7979
set_robot_config(robot_model, joint_list, joint_angles)
8080
target_coords = skrobot.coordinates.Coordinates([0.8, -0.6, 0.8], [0, 0, 0])
8181

82-
rarm_end_coords = skrobot.coordinates.CascadedCoords(
82+
right_arm_end_coords = skrobot.coordinates.CascadedCoords(
8383
parent=robot_model.r_gripper_tool_frame,
84-
name='rarm_end_coords')
84+
name='right_arm_end_coords')
8585
robot_model.inverse_kinematics(
8686
target_coords=target_coords,
8787
link_list=link_list,
88-
move_target=robot_model.rarm_end_coords, rotation_axis=True)
88+
move_target=robot_model.right_arm_end_coords, rotation_axis=True)
8989
av_goal = get_robot_config(robot_model, joint_list, with_base=with_base)
9090

9191
# collision checker setup
@@ -134,17 +134,17 @@
134134

135135
print("solving time : {0} sec".format(time.time() - ts))
136136

137-
rarm_point_history = []
137+
arm_point_history = []
138138
line_string = None
139139
for av in av_seq:
140140
set_robot_config(robot_model, joint_list, av, with_base=with_base)
141-
rarm_point_history.append(rarm_end_coords.worldpos())
141+
arm_point_history.append(right_arm_end_coords.worldpos())
142142

143-
# update rarm trajectory visualization
143+
# update arm trajectory visualization
144144
if line_string is not None:
145145
viewer.delete(line_string)
146-
if len(rarm_point_history) > 1:
147-
line_string = LineString(np.array(rarm_point_history))
146+
if len(arm_point_history) > 1:
147+
line_string = LineString(np.array(arm_point_history))
148148
viewer.add(line_string)
149149

150150
sscc.update_color()

examples/grasp_and_pull_box.py

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -25,22 +25,22 @@ def init_pose():
2525

2626

2727
def open_right_gripper():
28-
robot_model.gripper_distance(0.093, arm='rarm')
28+
robot_model.gripper_distance(0.093, arm='right_arm')
2929
viewer.redraw()
3030

3131

3232
def close_right_gripper():
33-
robot_model.gripper_distance(0.0, arm='rarm')
33+
robot_model.gripper_distance(0.0, arm='right_arm')
3434
viewer.redraw()
3535

3636

3737
def open_left_gripper():
38-
robot_model.gripper_distance(0.093, arm='larm')
38+
robot_model.gripper_distance(0.093, arm='left_arm')
3939
viewer.redraw()
4040

4141

4242
def close_left_gripper():
43-
robot_model.gripper_distance(0.0, arm='larm')
43+
robot_model.gripper_distance(0.0, arm='left_arm')
4444
viewer.redraw()
4545

4646

@@ -66,7 +66,7 @@ def move_to_box(box):
6666

6767

6868
def grasp_box(box):
69-
robot_model.gripper_distance(0.036, arm='rarm')
69+
robot_model.gripper_distance(0.036, arm='right_arm')
7070
move_target.assoc(box)
7171
viewer.redraw()
7272

@@ -109,10 +109,10 @@ def pull_box(box):
109109
robot_model.r_forearm_roll_link,
110110
robot_model.r_wrist_flex_link,
111111
robot_model.r_wrist_roll_link]
112-
rarm_end_coords = skrobot.coordinates.CascadedCoords(
112+
right_arm_end_coords = skrobot.coordinates.CascadedCoords(
113113
parent=robot_model.r_gripper_tool_frame,
114-
name='rarm_end_coords')
115-
move_target = rarm_end_coords
114+
name='right_arm_end_coords')
115+
move_target = right_arm_end_coords
116116

117117
# Create viewer
118118
if args.viewer == 'trimesh':

examples/notebooks/colab_jupyter_viewer_demo.ipynb

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -152,10 +152,10 @@
152152
" robot.r_wrist_roll_link]\n",
153153
"\n",
154154
"# Create end-effector coordinates\n",
155-
"rarm_end_coords = skrobot.coordinates.CascadedCoords(\n",
155+
"right_arm_end_coords = skrobot.coordinates.CascadedCoords(\n",
156156
" parent=robot.r_gripper_tool_frame,\n",
157-
" name='rarm_end_coords')\n",
158-
"move_target = rarm_end_coords\n",
157+
" name='right_arm_end_coords')\n",
158+
"move_target = right_arm_end_coords\n",
159159
"\n",
160160
"# Create viewer and add robot\n",
161161
"grasp_viewer = skrobot.viewers.JupyterNotebookViewer(height=600)\n",
@@ -193,7 +193,7 @@
193193
"robot.reset_pose()\n",
194194
"target_coords = skrobot.coordinates.Coordinates([0.5, -0.3, 0.7], [0, 0, 0])\n",
195195
"robot.inverse_kinematics(target_coords, link_list=link_list, move_target=move_target)\n",
196-
"robot.gripper_distance(0.093, arm='rarm') # Open gripper\n",
196+
"robot.gripper_distance(0.093, arm='right_arm') # Open gripper\n",
197197
"grasp_viewer.redraw()\n",
198198
"time.sleep(1)\n",
199199
"\n",
@@ -213,7 +213,7 @@
213213
"\n",
214214
"# 3. Grasp box\n",
215215
"print(\"3. Grasping box...\")\n",
216-
"robot.gripper_distance(0.036, arm='rarm') # Close gripper\n",
216+
"robot.gripper_distance(0.036, arm='right_arm') # Close gripper\n",
217217
"move_target.assoc(box) # Attach box to gripper\n",
218218
"grasp_viewer.redraw()\n",
219219
"time.sleep(1)\n",

examples/notebooks/jupyter_collision_free_with_base.ipynb

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -108,13 +108,13 @@
108108
"set_robot_config(robot_model, joint_list, joint_angles)\n",
109109
"target_coords = skrobot.coordinates.Coordinates([0.8, -0.6, 0.8], [0, 0, 0])\n",
110110
"\n",
111-
"rarm_end_coords = skrobot.coordinates.CascadedCoords(\n",
111+
"right_arm_end_coords = skrobot.coordinates.CascadedCoords(\n",
112112
" parent=robot_model.r_gripper_tool_frame,\n",
113-
" name='rarm_end_coords')\n",
113+
" name='right_arm_end_coords')\n",
114114
"robot_model.inverse_kinematics(\n",
115115
" target_coords=target_coords,\n",
116116
" link_list=link_list,\n",
117-
" move_target=robot_model.rarm_end_coords, rotation_axis=True)\n",
117+
" move_target=robot_model.right_arm_end_coords, rotation_axis=True)\n",
118118
"av_goal = get_robot_config(robot_model, joint_list, with_base=True)\n",
119119
"\n",
120120
"print(f\"Goal config (7 joints + 3 base): {av_goal}\")\n",
@@ -214,19 +214,19 @@
214214
"# Display viewer in THIS cell\n",
215215
"viewer.show()\n",
216216
"\n",
217-
"rarm_point_history = []\n",
217+
"arm_point_history = []\n",
218218
"line_string = None\n",
219219
"\n",
220220
"for i, av in enumerate(av_seq):\n",
221221
" # Update robot configuration (joints + base)\n",
222222
" set_robot_config(robot_model, joint_list, av, with_base=True)\n",
223-
" rarm_point_history.append(rarm_end_coords.worldpos())\n",
223+
" arm_point_history.append(right_arm_end_coords.worldpos())\n",
224224
"\n",
225225
" # Update trajectory visualization\n",
226226
" if line_string is not None:\n",
227227
" viewer.delete(line_string)\n",
228-
" if len(rarm_point_history) > 1:\n",
229-
" line_string = LineString(np.array(rarm_point_history))\n",
228+
" if len(arm_point_history) > 1:\n",
229+
" line_string = LineString(np.array(arm_point_history))\n",
230230
" viewer.add(line_string)\n",
231231
"\n",
232232
" # Update collision sphere colors\n",

examples/pr2_inverse_kinematics.py

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -19,8 +19,8 @@ def demonstrate_revert_if_fail(robot_model, target_coords, link_list, viewer,
1919
print("=" * 60)
2020

2121
# Get the correct end-effector position after first IK
22-
actual_end_pos = robot_model.rarm_end_coords.worldpos()
23-
actual_end_rot = robot_model.rarm_end_coords.worldrot()
22+
actual_end_pos = robot_model.right_arm_end_coords.worldpos()
23+
actual_end_rot = robot_model.right_arm_end_coords.worldrot()
2424

2525
# Visualize current end-effector position with green coordinate frame
2626
current_ee_axis = skrobot.model.Axis(
@@ -58,12 +58,12 @@ def demonstrate_revert_if_fail(robot_model, target_coords, link_list, viewer,
5858
result_standard = robot_model.inverse_kinematics(
5959
unreachable_coords,
6060
link_list=link_list,
61-
move_target=robot_model.rarm_end_coords,
61+
move_target=robot_model.right_arm_end_coords,
6262
rotation_axis=True,
6363
revert_if_fail=True # Default behavior
6464
)
6565

66-
standard_end_pos = robot_model.rarm_end_coords.worldpos()
66+
standard_end_pos = robot_model.right_arm_end_coords.worldpos()
6767
success_standard = result_standard is not False and result_standard is not None
6868
print(" Result: {}".format("Success" if success_standard else "Failed"))
6969
print(" End-effector position: {}".format(standard_end_pos))
@@ -73,7 +73,7 @@ def demonstrate_revert_if_fail(robot_model, target_coords, link_list, viewer,
7373
robot_model.inverse_kinematics(
7474
target_coords, # The original successful target
7575
link_list=link_list,
76-
move_target=robot_model.rarm_end_coords,
76+
move_target=robot_model.right_arm_end_coords,
7777
rotation_axis=True
7878
)
7979

@@ -86,21 +86,21 @@ def demonstrate_revert_if_fail(robot_model, target_coords, link_list, viewer,
8686
result_progressive = robot_model.inverse_kinematics(
8787
unreachable_coords,
8888
link_list=link_list,
89-
move_target=robot_model.rarm_end_coords,
89+
move_target=robot_model.right_arm_end_coords,
9090
rotation_axis=True,
9191
revert_if_fail=False # Keep partial progress
9292
)
9393
else:
9494
result_progressive = robot_model.inverse_kinematics(
9595
unreachable_coords,
9696
link_list=link_list,
97-
move_target=robot_model.rarm_end_coords,
97+
move_target=robot_model.right_arm_end_coords,
9898
rotation_axis=True,
9999
revert_if_fail=False # Keep partial progress
100100
)
101101

102-
progressive_end_pos = robot_model.rarm_end_coords.worldpos()
103-
progressive_end_rot = robot_model.rarm_end_coords.worldrot()
102+
progressive_end_pos = robot_model.right_arm_end_coords.worldpos()
103+
progressive_end_rot = robot_model.right_arm_end_coords.worldrot()
104104
success_progressive = result_progressive is not False and result_progressive is not None
105105
print(" Result: {}".format("Success" if success_progressive else "Failed (but kept progress)"))
106106
print(" End-effector position: {}".format(progressive_end_pos))
@@ -234,7 +234,7 @@ def main():
234234
# Build IK kwargs
235235
ik_kwargs = {
236236
'link_list': link_list,
237-
'move_target': robot_model.rarm_end_coords,
237+
'move_target': robot_model.right_arm_end_coords,
238238
'rotation_axis': True,
239239
}
240240
if translation_tolerance is not None:

examples/pybullet_robot_interface.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -51,8 +51,8 @@ def main():
5151
skrobot.interfaces.pybullet.draw(target_coords)
5252
robot.inverse_kinematics(
5353
target_coords,
54-
link_list=robot.rarm.link_list,
55-
move_target=robot.rarm_end_coords,
54+
link_list=robot.arm.link_list,
55+
move_target=robot.arm_end_coords,
5656
rotation_axis=True,
5757
stop=1000,
5858
)

skrobot/model/robot_model.py

Lines changed: 77 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1866,6 +1866,83 @@ def init_pose(self):
18661866
self.joint_max_angles)
18671867
return self.angle_vector(target_angles)
18681868

1869+
# New naming convention properties with more intuitive full names.
1870+
# These provide access using full names (right_arm, left_arm, etc.)
1871+
# while keeping the abbreviated names (rarm, larm, etc.) available for
1872+
# backward compatibility.
1873+
1874+
def _get_limb(self, attr_name):
1875+
"""Get limb by attribute name, returning None if not available."""
1876+
try:
1877+
limb = getattr(self, attr_name, None)
1878+
if isinstance(limb, RobotModel):
1879+
return limb
1880+
except NotImplementedError:
1881+
# Some robot models raise NotImplementedError for unsupported limbs
1882+
pass
1883+
return None
1884+
1885+
def _get_end_coords(self, attr_name):
1886+
"""Get end coords by attribute name, returning None if not available."""
1887+
try:
1888+
coords = getattr(self, attr_name, None)
1889+
if isinstance(coords, CascadedCoords):
1890+
return coords
1891+
except NotImplementedError:
1892+
# Some robot models raise NotImplementedError for unsupported coords
1893+
pass
1894+
return None
1895+
1896+
@property
1897+
def right_arm(self):
1898+
"""Right arm kinematic chain (alias for rarm)."""
1899+
return self._get_limb('rarm')
1900+
1901+
@property
1902+
def left_arm(self):
1903+
"""Left arm kinematic chain (alias for larm)."""
1904+
return self._get_limb('larm')
1905+
1906+
@property
1907+
def right_leg(self):
1908+
"""Right leg kinematic chain (alias for rleg)."""
1909+
return self._get_limb('rleg')
1910+
1911+
@property
1912+
def left_leg(self):
1913+
"""Left leg kinematic chain (alias for lleg)."""
1914+
return self._get_limb('lleg')
1915+
1916+
@property
1917+
def right_arm_end_coords(self):
1918+
"""Right arm end effector coordinates (alias for rarm_end_coords)."""
1919+
return self._get_end_coords('rarm_end_coords')
1920+
1921+
@property
1922+
def left_arm_end_coords(self):
1923+
"""Left arm end effector coordinates (alias for larm_end_coords)."""
1924+
return self._get_end_coords('larm_end_coords')
1925+
1926+
@property
1927+
def right_leg_end_coords(self):
1928+
"""Right leg end effector coordinates (alias for rleg_end_coords)."""
1929+
return self._get_end_coords('rleg_end_coords')
1930+
1931+
@property
1932+
def left_leg_end_coords(self):
1933+
"""Left leg end effector coordinates (alias for lleg_end_coords)."""
1934+
return self._get_end_coords('lleg_end_coords')
1935+
1936+
@property
1937+
def arm(self):
1938+
"""Arm kinematic chain for single-arm robots (alias for rarm)."""
1939+
return self._get_limb('rarm')
1940+
1941+
@property
1942+
def arm_end_coords(self):
1943+
"""Arm end effector coordinates for single-arm robots."""
1944+
return self._get_end_coords('rarm_end_coords')
1945+
18691946
def _meshes_from_urdf_visuals(self, visuals):
18701947
meshes = []
18711948
for visual in visuals:

skrobot/models/fetch.py

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -85,3 +85,16 @@ def rarm_with_torso(self):
8585
joint_list=rarm_with_torso_joints)
8686
r.end_coords = self.rarm_end_coords
8787
return r
88+
89+
# New naming convention aliases (backward compatible)
90+
@property
91+
def arm(self):
92+
return self.rarm
93+
94+
@property
95+
def arm_with_torso(self):
96+
return self.rarm_with_torso
97+
98+
@property
99+
def arm_end_coords(self):
100+
return self.rarm_end_coords

skrobot/models/kuka.py

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,3 +61,12 @@ def open_hand(self, default_angle=np.deg2rad(10), av=None):
6161
av[-2] = default_angle
6262
av[-4] = -default_angle
6363
return self.angle_vector(av)
64+
65+
# New naming convention aliases (backward compatible)
66+
@property
67+
def arm(self):
68+
return self.rarm
69+
70+
@property
71+
def arm_end_coords(self):
72+
return self.rarm_end_coords

0 commit comments

Comments
 (0)