Skip to content

Commit 9a611c1

Browse files
committed
new examples
1 parent 4859a35 commit 9a611c1

File tree

4 files changed

+173
-6
lines changed

4 files changed

+173
-6
lines changed

examples/baur.py

Lines changed: 92 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,92 @@
1+
#!/usr/bin/env python
2+
"""
3+
@author Jesse Haviland
4+
"""
5+
6+
import roboticstoolbox as rtb
7+
import spatialmath as sm
8+
import numpy as np
9+
10+
"""
11+
This is an implementation of the controller from:
12+
J. Baur, J. Pfaff, H. Ulbrich, and T. Villgrattner, “Design and development
13+
of a redundant modular multipurpose agricultural manipulator,” in 2012
14+
IEEE/ASME International Conference on Advanced Intelligent Mechatronics
15+
(AIM), 2012, pp. 823–830.
16+
"""
17+
18+
# Launch the simulator Swift
19+
env = rtb.backend.Swift()
20+
env.launch()
21+
22+
# Create a Panda robot object
23+
panda = rtb.models.Panda()
24+
25+
# Set joint angles to ready configuration
26+
panda.q = panda.qr
27+
28+
# Add the Panda to the simulator
29+
env.add(panda)
30+
31+
# Number of joint in the panda which we are controlling
32+
n = 7
33+
34+
# Set the desired end-effector pose
35+
Tep = panda.fkine() * sm.SE3(0.3, 0.2, 0.3)
36+
37+
arrived = False
38+
39+
while not arrived:
40+
41+
# The pose of the Panda's end-effector
42+
Te = panda.fkine()
43+
44+
# The manipulator Jacobian in the end-effecotr frame
45+
Je = panda.jacobe()
46+
47+
# Calulate the required end-effector spatial velocity for the robot
48+
# to approach the goal. Gain is set to 1.0
49+
v, arrived = rtb.p_servo(Te, Tep, 1.0)
50+
51+
# Gain term (lambda) for control minimisation
52+
Y = 0.01
53+
54+
# The manipulability Jacobian
55+
Jm = panda.jacobm()
56+
57+
# The minimum angle (in radians) in which the joint is allowed to approach
58+
# to its limit
59+
ps = 0.05
60+
61+
# The influence angle (in radians) in which the velocity damper
62+
# becomes active
63+
pi = 0.9
64+
65+
# Add cost to going in the direction of joint limits, if they are within
66+
# the influence distance
67+
b = np.zeros((n, 1))
68+
69+
for i in range(n):
70+
if panda.q[i] - panda.qlim[0, i] <= pi:
71+
b[i, 0] = -1 * \
72+
np.power(((panda.q[i] - panda.qlim[0, i]) - pi), 2) \
73+
/ np.power((ps - pi), 2)
74+
if panda.qlim[1, i] - panda.q[i] <= pi:
75+
b[i, 0] = 1 * \
76+
np.power(((panda.qlim[1, i] - panda.q[i]) - pi), 2) \
77+
/ np.power((ps - pi), 2)
78+
79+
# Project the gradient of manipulability into the null-space of the
80+
# differential kinematics
81+
null = (
82+
np.eye(n) - np.linalg.pinv(Je) @ Je
83+
) @ (Jm - b)
84+
85+
# Solve for the joint velocities dq
86+
qd = np.linalg.pinv(Je) @ v + 1 / Y * null.flatten()
87+
88+
# Apply the joint velocities to the Panda
89+
panda.qd[:n] = qd[:n]
90+
91+
# Step the simulator by 50 ms
92+
env.step(50)

examples/mmc.py

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,12 @@
88
import numpy as np
99
import qpsolvers as qp
1010

11+
"""
12+
This is an implementation of the controller from:
13+
J. Haviland and P. Corke, “A purely-reactive manipulability-maximising
14+
motion controller,” arXiv preprint arXiv:2002.11901,2020.
15+
"""
16+
1117
# Launch the simulator Swift
1218
env = rtb.backend.Swift()
1319
env.launch()

examples/park.py

Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
1+
#!/usr/bin/env python
2+
"""
3+
@author Jesse Haviland
4+
"""
5+
6+
import roboticstoolbox as rtb
7+
import spatialmath as sm
8+
import numpy as np
9+
10+
"""
11+
This is an implementation of the controller from:
12+
Park, C. Wangkyun, and Y. Youngil, “Computation of gradientof manipulability
13+
for kinematically redundant manipulators includingdual manipulators system,”
14+
Transactions on Control, Automation and Systems Engineering, vol. 1, no. 1,
15+
pp. 8–15, 1999.
16+
"""
17+
18+
# Launch the simulator Swift
19+
env = rtb.backend.Swift()
20+
env.launch()
21+
22+
# Create a Panda robot object
23+
panda = rtb.models.Panda()
24+
25+
# Set joint angles to ready configuration
26+
panda.q = panda.qr
27+
28+
# Add the Panda to the simulator
29+
env.add(panda)
30+
31+
# Number of joint in the panda which we are controlling
32+
n = 7
33+
34+
# Set the desired end-effector pose
35+
Tep = panda.fkine() * sm.SE3(0.3, 0.2, 0.3)
36+
37+
arrived = False
38+
39+
while not arrived:
40+
41+
# The pose of the Panda's end-effector
42+
Te = panda.fkine()
43+
44+
# The manipulator Jacobian in the end-effecotr frame
45+
Je = panda.jacobe()
46+
47+
# Calulate the required end-effector spatial velocity for the robot
48+
# to approach the goal. Gain is set to 1.0
49+
v, arrived = rtb.p_servo(Te, Tep, 1.0)
50+
51+
# Gain term (lambda) for control minimisation
52+
Y = 0.01
53+
54+
# The manipulability Jacobian
55+
Jm = panda.jacobm()
56+
57+
# Project the gradient of manipulability into the null-space of the
58+
# differential kinematics
59+
null = (
60+
np.eye(n) - np.linalg.pinv(Je) @ Je
61+
) @ Jm
62+
63+
# Solve for the joint velocities dq
64+
qd = np.linalg.pinv(Je) @ v + 1 / Y * null.flatten()
65+
66+
# Apply the joint velocities to the Panda
67+
panda.qd[:n] = qd[:n]
68+
69+
# Step the simulator by 50 ms
70+
env.step(50)

roboticstoolbox/backend/Swift/Swift.py

Lines changed: 5 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -177,12 +177,11 @@ def add(self, ob, show_robot=True, show_collision=False):
177177

178178
self.robots.append(ob)
179179
return id
180-
# elif isinstance(ob, rp.Shape):
181-
# shape = ob.to_dict()
182-
# id = self.swift.shape(shape)
183-
# id = self._send_socket('shape', shape)
184-
# self.shapes.append(ob)
185-
# return id
180+
elif isinstance(ob, rp.Shape):
181+
shape = ob.to_dict()
182+
id = self._send_socket('shape', shape)
183+
self.shapes.append(ob)
184+
return id
186185

187186
def remove(self):
188187
"""

0 commit comments

Comments
 (0)
pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy