|
49 | 49 | env.add(target) |
50 | 50 |
|
51 | 51 | # Set the desired end-effector pose to the location of target |
52 | | -Tep = panda.fkine() |
| 52 | +Tep = panda.fkine(panda.q) |
53 | 53 | Tep.A[:3, 3] = target.base.t |
54 | 54 | Tep.A[2, 3] += 0.1 |
55 | 55 |
|
|
58 | 58 | while not arrived: |
59 | 59 |
|
60 | 60 | # The pose of the Panda's end-effector |
61 | | - Te = panda.fkine() |
| 61 | + Te = panda.fkine(panda.q) |
62 | 62 |
|
63 | 63 | # Transform from the end-effector to desired pose |
64 | 64 | eTep = Te.inv() * Tep |
|
83 | 83 | Q[n:, n:] = (1 / e) * np.eye(6) |
84 | 84 |
|
85 | 85 | # The equality contraints |
86 | | - Aeq = np.c_[panda.jacobe(), np.eye(6)] |
| 86 | + Aeq = np.c_[panda.jacobe(panda.q), np.eye(6)] |
87 | 87 | beq = v.reshape((6,)) |
88 | 88 |
|
89 | 89 | # The inequality constraints for joint limit avoidance |
|
108 | 108 | # object on the robot to the collision in the scene |
109 | 109 | c_Ain, c_bin = panda.link_collision_damper( |
110 | 110 | collision, panda.q[:n], 0.3, 0.05, 1.0, |
111 | | - panda.link_dict['panda_link1'], panda.link_dict['panda_hand']) |
| 111 | + startlink=panda.link_dict['panda_link1'], |
| 112 | + endlink=panda.link_dict['panda_hand']) |
112 | 113 |
|
113 | 114 | # If there are any parts of the robot within the influence distance |
114 | 115 | # to the collision in the scene |
|
120 | 121 | bin = np.r_[bin, c_bin] |
121 | 122 |
|
122 | 123 | # Linear component of objective function: the manipulability Jacobian |
123 | | - c = np.r_[-panda.jacobm().reshape((n,)), np.zeros(6)] |
| 124 | + c = np.r_[-panda.jacobm(panda.q).reshape((n,)), np.zeros(6)] |
124 | 125 |
|
125 | 126 | # The lower and upper bounds on the joint velocity and slack variable |
126 | 127 | lb = -np.r_[panda.qdlim[:n], 10 * np.ones(6)] |
|
0 commit comments