|
9 | 9 | # IK |
10 | 10 | from spatialmath import SE3 |
11 | 11 |
|
12 | | -T = SE3(0.8, 0.2, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1]) |
13 | | -q_pickup, *_ = robot.ikine(T) # solve IK, ignore additional outputs |
14 | | -print(q_pickup)# display joint angles |
15 | | -print(robot.fkine(q_pickup)) # FK shows that desired end-effector pose was achieved |
| 12 | +T = SE3(0.7, 0.2, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1]) |
| 13 | +sol = robot.ikine_LMS(T) # solve IK, ignore additional outputs |
| 14 | +print(sol.q) # display joint angles |
| 15 | +print(robot.fkine(sol.q)) # FK shows that desired end-effector pose was achieved |
16 | 16 |
|
17 | 17 |
|
18 | | -qt = rtb.trajectory.jtraj(robot.qz, q_pickup, 50) |
19 | | -robot.plot(qt.q, movie="panda1.gif") |
| 18 | +qtraj = rtb.jtraj(robot.qz, sol.q, 50) |
| 19 | +robot.plot(qtraj.q, movie="panda1.gif") |
20 | 20 |
|
21 | 21 | # URDF + Swift version |
22 | 22 | dt = 0.050 # simulation timestep in seconds |
|
27 | 27 | env.launch("chrome") # activate it |
28 | 28 | env.add(robot) # add robot to the 3D scene |
29 | 29 | env.start_recording("panda2", 1 / dt) |
30 | | -for qk in qt.q: # for each joint configuration on trajectory |
| 30 | +for qk in qtraj.q: # for each joint configuration on trajectory |
31 | 31 | robot.q = qk # update the robot state |
32 | 32 | env.step() # update visualization |
33 | 33 | env.stop_recording() |
|
0 commit comments