|
7 | 7 | import spatialmath as sm |
8 | 8 | import numpy as np |
9 | 9 | import qpsolvers as qp |
10 | | -import time |
11 | 10 |
|
12 | 11 | # Launch the simulator Swift |
13 | 12 | env = rtb.backend.Swift() |
|
17 | 16 | panda = rtb.models.Panda() |
18 | 17 |
|
19 | 18 | # Set joint angles to ready configuration |
20 | | -# panda.q = [ |
21 | | - # -0.5653, -0.1941, -1.2602, -0.7896, -2.3227, -0.3919, -2.5173, 0.0, 0.0] |
22 | 19 | panda.q = panda.qr |
23 | 20 |
|
24 | 21 | # Number of joint in the panda which we are controlling |
|
27 | 24 | # Make two obstacles with velocities |
28 | 25 | s0 = rtb.Sphere( |
29 | 26 | radius=0.05, |
30 | | - base=sm.SE3(0.45, 0.4, 0.3) |
| 27 | + base=sm.SE3(0.52, 0.4, 0.3) |
31 | 28 | ) |
32 | | -s0.v = [0.01, -0.2, 0, 0, 0, 0] |
| 29 | +s0.v = [0, -0.2, 0, 0, 0, 0] |
33 | 30 |
|
34 | 31 | s1 = rtb.Sphere( |
35 | 32 | radius=0.05, |
|
89 | 86 | Aeq = np.c_[panda.jacobe(), np.eye(6)] |
90 | 87 | beq = v.reshape((6,)) |
91 | 88 |
|
92 | | - # # The inequality constraints for joint limit avoidance |
93 | | - # Ain = np.zeros((n + 6, n + 6)) |
94 | | - # bin = np.zeros(n + 6) |
| 89 | + # The inequality constraints for joint limit avoidance |
| 90 | + Ain = np.zeros((n + 6, n + 6)) |
| 91 | + bin = np.zeros(n + 6) |
95 | 92 |
|
96 | | -# # The minimum angle (in radians) in which the joint is allowed to approach |
97 | | -# # to its limit |
98 | | -# ps = 0.05 |
| 93 | + # The minimum angle (in radians) in which the joint is allowed to approach |
| 94 | + # to its limit |
| 95 | + ps = 0.05 |
99 | 96 |
|
100 | | -# # The influence angle (in radians) in which the velocity damper |
101 | | -# # becomes active |
102 | | -# pi = 0.9 |
| 97 | + # The influence angle (in radians) in which the velocity damper |
| 98 | + # becomes active |
| 99 | + pi = 0.9 |
103 | 100 |
|
104 | | -# # Form the joint limit velocity damper |
105 | | -# Ain[:n, :n], bin[:n] = panda.joint_velocity_damper(ps, pi, n) |
106 | | - |
107 | | - Ain = None |
108 | | - bin = None |
| 101 | + # Form the joint limit velocity damper |
| 102 | + Ain[:n, :n], bin[:n] = panda.joint_velocity_damper(ps, pi, n) |
109 | 103 |
|
| 104 | + # For each collision in the scene |
110 | 105 | for collision in collisions: |
111 | 106 |
|
| 107 | + # Form the velocity damper inequality contraint for each collision |
| 108 | + # object on the robot to the collision in the scene |
112 | 109 | c_Ain, c_bin = panda.link_collision_damper( |
113 | | - collision, panda.q[:n], 0.3, 0.05, |
| 110 | + collision, panda.q[:n], 0.3, 0.05, 1.0, |
114 | 111 | panda.elinks['panda_joint1'], panda.elinks['panda_hand_joint']) |
115 | 112 |
|
| 113 | + # If there are any parts of the robot within the influence distance |
| 114 | + # to the collision in the scene |
116 | 115 | if c_Ain is not None and c_bin is not None: |
117 | 116 | c_Ain = np.c_[c_Ain, np.zeros((c_Ain.shape[0], 6))] |
118 | 117 |
|
119 | | - if Ain is None: |
120 | | - Ain = c_Ain |
121 | | - else: |
122 | | - Ain = np.r_[Ain, c_Ain] |
123 | | - |
124 | | - if bin is None: |
125 | | - bin = np.array(c_bin) |
126 | | - else: |
127 | | - bin = np.r_[bin, c_bin] |
| 118 | + # Stack the inequality constraints |
| 119 | + Ain = np.r_[Ain, c_Ain] |
| 120 | + bin = np.r_[bin, c_bin] |
128 | 121 |
|
129 | 122 | # Linear component of objective function: the manipulability Jacobian |
130 | 123 | c = np.r_[-panda.jacobm().reshape((n,)), np.zeros(6)] |
|
0 commit comments