-
Notifications
You must be signed in to change notification settings - Fork 32
Expand file tree
/
Copy pathpr2_inverse_kinematics.py
More file actions
executable file
·387 lines (336 loc) · 14.3 KB
/
Copy pathpr2_inverse_kinematics.py
File metadata and controls
executable file
·387 lines (336 loc) · 14.3 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
#!/usr/bin/env python
import argparse
import time
import numpy as np
import skrobot
from skrobot.utils.visualization import ik_visualization
def demonstrate_revert_if_fail(robot_model, target_coords, joint_list, viewer,
no_ik_visualization):
"""Demonstrate the difference between revert_if_fail=True and False"""
import numpy as np
print("\n" + "=" * 60)
print("Testing revert_if_fail=False with unreachable target")
print("=" * 60)
# Get the correct end-effector position after first IK
actual_end_pos = robot_model.right_arm_end_coords.worldpos()
actual_end_rot = robot_model.right_arm_end_coords.worldrot()
# Visualize current end-effector position with green coordinate frame
current_ee_axis = skrobot.model.Axis(
axis_radius=0.008,
axis_length=0.12,
pos=actual_end_pos,
rot=actual_end_rot
)
# Set green color for current end-effector
current_ee_axis.set_color([0, 255, 0]) # RGB for green
viewer.add(current_ee_axis)
# Define an unreachable target (too far away)
unreachable_pos = [0.8, -0.8, 1.2] # Beyond robot's reach
print("Attempting unreachable target: {}".format(unreachable_pos))
print("Current end-effector position: {}".format(actual_end_pos))
unreachable_coords = skrobot.coordinates.Coordinates(unreachable_pos, [0, 0, 0])
# Visualize unreachable target with a different color (red)
unreachable_axis = skrobot.model.Axis(
axis_radius=0.012,
axis_length=0.18,
pos=unreachable_coords.translation,
rot=unreachable_coords.rotation
)
# Set red color for unreachable target
unreachable_axis.set_color([255, 0, 0]) # RGB for red
viewer.add(unreachable_axis)
viewer.redraw()
print("Unreachable target visualized with red coordinate frame")
# First attempt with default revert_if_fail=True
print("\n1. Standard IK (revert_if_fail=True):")
result_standard = robot_model.inverse_kinematics(
unreachable_coords,
joint_list=joint_list,
move_target=robot_model.right_arm_end_coords,
rotation_mask=True,
revert_if_fail=True # Default behavior
)
standard_end_pos = robot_model.right_arm_end_coords.worldpos()
success_standard = result_standard is not False and result_standard is not None
print(" Result: {}".format("Success" if success_standard else "Failed"))
print(" End-effector position: {}".format(standard_end_pos))
print(" Distance to target: {:.3f}m".format(np.linalg.norm(standard_end_pos - unreachable_pos)))
# Reset to a known good position (the successful target from earlier)
robot_model.inverse_kinematics(
target_coords, # The original successful target
joint_list=joint_list,
move_target=robot_model.right_arm_end_coords,
rotation_mask=True
)
viewer.redraw()
# Second attempt with revert_if_fail=False
print("\n2. Progressive IK (revert_if_fail=False):")
if not no_ik_visualization:
with ik_visualization(viewer, sleep_time=0.5):
result_progressive = robot_model.inverse_kinematics(
unreachable_coords,
joint_list=joint_list,
move_target=robot_model.right_arm_end_coords,
rotation_mask=True,
revert_if_fail=False # Keep partial progress
)
else:
result_progressive = robot_model.inverse_kinematics(
unreachable_coords,
joint_list=joint_list,
move_target=robot_model.right_arm_end_coords,
rotation_mask=True,
revert_if_fail=False # Keep partial progress
)
progressive_end_pos = robot_model.right_arm_end_coords.worldpos()
progressive_end_rot = robot_model.right_arm_end_coords.worldrot()
success_progressive = result_progressive is not False and result_progressive is not None
print(" Result: {}".format("Success" if success_progressive else "Failed (but kept progress)"))
print(" End-effector position: {}".format(progressive_end_pos))
print(" Distance to target: {:.3f}m".format(np.linalg.norm(progressive_end_pos - unreachable_pos)))
# Visualize progressive IK result with cyan coordinate frame
progressive_result_axis = skrobot.model.Axis(
axis_radius=0.006,
axis_length=0.10,
pos=progressive_end_pos,
rot=progressive_end_rot
)
# Set cyan color for progressive IK result
progressive_result_axis.set_color([0, 255, 255]) # RGB for cyan
viewer.add(progressive_result_axis)
# Compare distances
standard_distance = np.linalg.norm(standard_end_pos - unreachable_pos)
progressive_distance = np.linalg.norm(progressive_end_pos - unreachable_pos)
print("\n" + "-" * 50)
print("Comparison:")
print(" Standard IK distance to target: {:.3f}m".format(standard_distance))
print(" Progressive IK distance to target: {:.3f}m".format(progressive_distance))
if progressive_distance < standard_distance:
improvement = standard_distance - progressive_distance
print(" [OK] Progressive IK got {:.3f}m closer to target!".format(improvement))
else:
print(" Standard IK performed better in this case")
viewer.redraw()
print("\nAll IK demonstrations completed!")
print("\n" + "=" * 60)
print("VISUALIZATION LEGEND:")
print("=" * 60)
print("Red coordinate frame : Unreachable target")
print("Cyan coordinate frame : Progressive IK result (revert_if_fail=False)")
print(" Frame axes: Red=X, Green=Y, Blue=Z")
print("=" * 60)
def demonstrate_fullbody_ik(robot_model, joint_list, viewer,
no_ik_visualization):
"""Demonstrate fullbody IK (use_base) reaching a target that is
out of arm range without moving the mobile base."""
print("\n" + "=" * 60)
print("Fullbody IK: reaching a far target by moving the base")
print("=" * 60)
# Reset to a clean pose so the demo starts deterministic.
robot_model.reset_pose()
viewer.redraw()
ee_pos0 = robot_model.right_arm_end_coords.worldpos().copy()
base_pos0 = robot_model.root_link.worldpos().copy()
# Target 0.8 m forward from the current end-effector pose — beyond
# the right arm's reach without base motion.
far_target_pos = ee_pos0 + np.array([0.8, 0.0, 0.0])
far_target = skrobot.coordinates.Coordinates(far_target_pos, [0, 0, 0])
print("Far target position: {}".format(far_target_pos))
print("Initial base position: {}".format(base_pos0))
far_target_axis = skrobot.model.Axis(
axis_radius=0.012,
axis_length=0.18,
pos=far_target.translation,
rot=far_target.rotation,
)
far_target_axis.set_color([255, 128, 0]) # orange
viewer.add(far_target_axis)
viewer.redraw()
# 1) Arm-only IK — expected to fail because target is out of reach.
print("\n1. Arm-only IK (use_base=False):")
result_armonly = robot_model.inverse_kinematics(
far_target,
joint_list=joint_list,
move_target=robot_model.right_arm_end_coords,
rotation_mask=False,
stop=100,
)
success_armonly = result_armonly is not False \
and result_armonly is not None
ee_armonly = robot_model.right_arm_end_coords.worldpos()
err_armonly = np.linalg.norm(ee_armonly - far_target_pos)
print(" Result: {}".format(
"Success" if success_armonly else "Failed"))
print(" End-effector position: {}".format(ee_armonly))
print(" Distance to target: {:.3f}m".format(err_armonly))
# Reset between attempts.
robot_model.reset_pose()
viewer.redraw()
# 2) Fullbody IK with planar base — base translates/rotates on
# the ground so the arm can reach.
print("\n2. Fullbody IK (use_base='planar'):")
if not no_ik_visualization:
with ik_visualization(viewer, sleep_time=0.5):
result_fullbody = robot_model.inverse_kinematics(
far_target,
joint_list=joint_list,
move_target=robot_model.right_arm_end_coords,
rotation_mask=False,
stop=100,
use_base='planar',
)
else:
result_fullbody = robot_model.inverse_kinematics(
far_target,
joint_list=joint_list,
move_target=robot_model.right_arm_end_coords,
rotation_mask=False,
stop=100,
use_base='planar',
)
success_fullbody = result_fullbody is not False \
and result_fullbody is not None
ee_fullbody = robot_model.right_arm_end_coords.worldpos()
base_fullbody = robot_model.root_link.worldpos()
err_fullbody = np.linalg.norm(ee_fullbody - far_target_pos)
base_moved = np.linalg.norm(base_fullbody - base_pos0)
print(" Result: {}".format(
"Success" if success_fullbody else "Failed"))
print(" End-effector position: {}".format(ee_fullbody))
print(" Distance to target: {:.3f}m".format(err_fullbody))
print(" Base moved by: {:.3f}m".format(base_moved))
viewer.redraw()
print("\n" + "-" * 50)
print("Comparison:")
print(" Arm-only distance to target: {:.3f}m".format(err_armonly))
print(" Fullbody distance to target: {:.3f}m".format(err_fullbody))
if err_fullbody < err_armonly:
print(" [OK] Fullbody IK reached the target by moving the base.")
print("\n" + "=" * 60)
print("VISUALIZATION LEGEND (fullbody demo):")
print("=" * 60)
print("Orange coordinate frame: Far target (out of arm-only reach)")
print("=" * 60)
def main():
parser = argparse.ArgumentParser(
description='Simple PR2 inverse kinematics example.')
parser.add_argument(
'--no-interactive',
action='store_true',
help="Run in non-interactive mode (do not wait for user input)"
)
parser.add_argument(
'--viewer', type=str,
choices=['trimesh', 'pyrender', 'viser'], default='pyrender',
help='Choose the viewer type: trimesh, pyrender or viser')
parser.add_argument(
'--no-ik-visualization',
action='store_true',
help="Disable inverse kinematics visualization during solving"
)
parser.add_argument(
'--skip-revert-demo',
action='store_true',
help="Skip the revert_if_fail demonstration"
)
parser.add_argument(
'--skip-fullbody-demo',
action='store_true',
help="Skip the fullbody IK (use_base) demonstration"
)
parser.add_argument(
'--translation-tolerance', '--translation_tolerance',
type=float, nargs=3, default=None, metavar=('X', 'Y', 'Z'),
help='Translation tolerance per axis in meters (e.g., 0.05 0.05 0.05)'
)
parser.add_argument(
'--rotation-tolerance', '--rotation_tolerance',
type=float, nargs=3, default=None, metavar=('R', 'P', 'Y'),
help='Rotation tolerance per axis in degrees (e.g., 10 10 10)'
)
args = parser.parse_args()
# Process tolerance arguments
translation_tolerance = args.translation_tolerance
rotation_tolerance = None
if args.rotation_tolerance is not None:
rotation_tolerance = [np.deg2rad(v) for v in args.rotation_tolerance]
# Create robot model
robot_model = skrobot.models.PR2()
robot_model.reset_pose()
# Define the joints to actuate for the right arm. Passing joints
# (joint_list) is the recommended, most direct way to say which joints
# move; link_list is the equivalent legacy spelling.
joint_list = [
robot_model.r_shoulder_pan_joint,
robot_model.r_shoulder_lift_joint,
robot_model.r_upper_arm_roll_joint,
robot_model.r_elbow_flex_joint,
robot_model.r_forearm_roll_joint,
robot_model.r_wrist_flex_joint,
robot_model.r_wrist_roll_joint
]
# Create viewer
viewer = skrobot.viewers.create_viewer(
args.viewer, resolution=(640, 480))
viewer.add(robot_model)
viewer.show()
viewer.set_camera([np.deg2rad(45), -np.deg2rad(0),
np.deg2rad(135)], distance=2.5)
# Define target position
target_pos = [0.7, -0.2, 0.8]
print("Solving inverse kinematics for target: {}".format(target_pos))
if translation_tolerance is not None:
print("Translation tolerance: {}m".format(translation_tolerance))
if args.rotation_tolerance is not None:
print("Rotation tolerance: {} degrees".format(args.rotation_tolerance))
# Create target coordinates
target_coords = skrobot.coordinates.Coordinates(target_pos, [0, 0, 0])
# Visualize target position with Axis model
target_axis = skrobot.model.Axis(
axis_radius=0.01,
axis_length=0.15,
pos=target_coords.translation,
rot=target_coords.rotation
)
viewer.add(target_axis)
viewer.redraw()
print("Target position visualized with coordinate frame")
# Build IK kwargs
ik_kwargs = {
'joint_list': joint_list,
'move_target': robot_model.right_arm_end_coords,
'rotation_axis': True,
}
if translation_tolerance is not None:
ik_kwargs['translation_tolerance'] = translation_tolerance
if rotation_tolerance is not None:
ik_kwargs['rotation_tolerance'] = rotation_tolerance
# Solve inverse kinematics with optional visualization
if not args.no_ik_visualization:
with ik_visualization(viewer, sleep_time=0.5):
result = robot_model.inverse_kinematics(target_coords, **ik_kwargs)
else:
result = robot_model.inverse_kinematics(target_coords, **ik_kwargs)
# Check if result is successful (could be False or an array)
success = result is not False and result is not None
if success:
print("Successfully reached target!")
else:
print("Failed to reach target")
viewer.redraw()
print("First IK solving completed!")
# Demonstrate revert_if_fail=False with an unreachable target
if not args.skip_revert_demo:
demonstrate_revert_if_fail(robot_model, target_coords, joint_list, viewer,
args.no_ik_visualization)
# Demonstrate fullbody IK with a far target
if not args.skip_fullbody_demo:
demonstrate_fullbody_ik(robot_model, joint_list, viewer,
args.no_ik_visualization)
if not args.no_interactive:
viewer.wait_until_close()
else:
viewer.close()
time.sleep(1.0)
if __name__ == '__main__':
main()