10000 Merge branch 'master' of github.com:petercorke/robotics-toolbox-python · yobzhuu/robotics-toolbox-python@67c49b2 · GitHub
[go: up one dir, main page]
Skip to content

Commit 67c49b2

Browse files
jhavljhavl
committed
Merge branch 'master' of github.com:petercorke/robotics-toolbox-python
2 parents 2c66c69 + 50e795c commit 67c49b2

File tree

10 files changed

+1211
-913
lines changed

10 files changed

+1211
-913
lines changed

‎docs/source/arm_dh.rst

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@ DHRobot
1414

1515
The various :ref:`DH Models` all subclass this class.
1616

17-
.. automodule:: roboticstoolbox.robot.DHRobot.DHRobot
17+
.. autoclass:: roboticstoolbox.robot.DHRobot.DHRobot
1818
:members:
1919
:undoc-members:
2020
:show-inheritance:

‎docs/source/conf.py

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -90,4 +90,10 @@
9090
# extensions = ['rst2pdf.pdfbuilder']
9191
# pdf_documents = [('index', u'rst2pdf', u'Sample rst2pdf doc', u'Your Name'),]
9292
autorun_languages = {}
93-
autorun_languages['pycon_output_encoding'] = 'UTF-8'
93+
autorun_languages['pycon_output_encoding'] = 'UTF-8'
94+
autorun_languages['pycon_initial_code'] = [
95+
"from spatialmath import SE3",
96+
"SE3._color = False",
97+
"import numpy as np",
98+
"np.set_printoptions(precision=4, suppress=True)",
99+
]

‎roboticstoolbox/robot/DHDynamics.py

Lines changed: 118 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,11 +12,24 @@
1212
"""
1313
import copy
1414
from collections import namedtuple
15+
from functools import wraps
1516
import numpy as np
1617
from spatialmath.base import \
1718
getvector, verifymatrix, isscalar, getmatrix, t2r
1819
from scipy import integrate, interpolate
1920
from spatialmath.base import symbolic as sym
21+
from frne import init, frne, delete
22+
23+
def _check_rne(func):
24+
@wraps(func)
25+
def wrapper_check_rne(*args, **kwargs):
26+
if args[0]._rne_ob is None or args[0]._dynchanged:
27+
args[0].delete_rne()
28+
args[0]._init_rne()
29+
args[0]._rne_changed = False
30+
return func(*args, **kwargs)
31+
return wrapper_check_rne
32+
2033

2134

2235
class DHDynamics:
@@ -32,6 +45,110 @@ def printdyn(self):
3245
print("\nLink {:d}::".format(j), link)
3346
print(link.dyn(indent=2))
3447

48+
def delete_rne(self):
49+
"""
50+
Frees the memory holding the robot object in c if the robot object
51+
has been initialised in c.
52+
"""
5 8000 3+
if self._rne_ob is not None:
54+
delete(self._rne_ob)
55+
self._dynchanged = False
56+
self._rne_ob = None
57+
58+
def _init_rne(self):
59+
# Compress link data into a 1D array
60+
L = np.zeros(24 * self.n)
61+
62+
for i in range(self.n):
63+
j = i * 24
64+
L[j] = self.links[i].alpha
65+
L[j + 1] = self.links[i].a
66+
L[j + 2] = self.links[i].theta
67+
L[j + 3] = self.links[i].d
68+
L[j + 4] = self.links[i].sigma
69+
L[j + 5] = self.links[i].offset
70+
L[j + 6] = self.links[i].m
71+
L[j + 7:j + 10] = self.links[i].r.flatten()
72+
L[j + 10:j + 19] = self.links[i].I.flatten()
73+
L[j + 19] = self.links[i].Jm
74+
L[j + 20] = self.links[i].G
75+
L[j + 21] = self.links[i].B
76+
L[j + 22:j + 24] = self.links[i].Tc.flatten()
77+
78+
self._rne_ob = init(self.n, self.mdh, L, self.gravity)
79+
80+
81+
@_check_rne
82+
def rne(self, q, qd=None, qdd=None, grav=None, fext=None):
83+
r"""
84+
Inverse dynamics
85+
86+
:param q: The joint angles/configuration of the robot (Optional,
87+
if not supplied will use the stored q values).
88+
:type q: float ndarray(n)
89+
:param qd: The joint velocities of the robot
90+
:type qd: float ndarray(n)
91+
:param qdd: The joint accelerations of the robot
92+
:type qdd: float ndarray(n)
93+
:param grav: Gravity vector to overwrite robots gravity value
94+
:type grav: float ndarray(6)
95+
:param fext: Specify wrench acting on the end-effector
96+
:math:`W=[F_x F_y F_z M_x M_y M_z]`
97+
:type fext: float ndarray(6)
98+
99+
``tau = rne(q, qd, qdd, grav, fext)`` is the joint torque required for
100+
the robot to achieve the specified joint position ``q`` (1xn), velocity
101+
``qd`` (1xn) and acceleration ``qdd`` (1xn), where n is the number of
102+
robot joints. ``fext`` describes the wrench acting on the end-effector
103+
104+
Trajectory operation:
105+
If q, qd and qdd (mxn) are matrices with m cols representing a
106+
trajectory then tau (mxn) is a matrix with cols corresponding to each
107+
trajectory step.
108+
109+
:notes:
110+
- The torque computed contains a contribution due to armature
111+
inertia and joint friction.
112+
- If a model has no dynamic parameters set the result is zero.
113+
114+
"""
115+
trajn = 1
116+
117+
try:
118+
q = getvector(q, self.n, 'row')
119+
qd = getvector(qd, self.n, 'row')
120+
qdd = getvector(qdd, self.n, 'row')
121+
except ValueError:
122+
trajn = q.shape[0]
123+
verifymatrix(q, (trajn, self.n))
124+
verifymatrix(qd, (trajn, self.n))
125+
verifymatrix(qdd, (trajn, self.n))
126+
127+
if grav is None:
128+
grav = self.gravity
129+
else:
130+
grav = getvector(grav, 3)
131+
132+
# The c function doesn't handle base rotation, so we need to hack the
133+
# gravity vector instead
134+
grav = self.base.R.T @ grav
135+
136+
if fext is None:
137+
fext = np.zeros(6)
138+
else:
139+
fext = getvector(fext, 6)
140+
141+
tau = np.zeros((trajn, self.n))
142+
143+
for i in range(trajn):
144+
tau[i, :] = frne(
145+
self._rne_ob, q[i, :], qd[i, :], qdd[i, :], grav, fext)
146+
147+
if trajn == 1:
148+
return tau[0, :]
149+
else:
150+
return tau
151+
35152
def fdyn(
36153
self, T, q0, torqfun=None, targs=None, qd0=None,
37154
solver='RK45', sargs=None, dt=None, progress=False):
@@ -1079,7 +1196,7 @@ def perturb(self, p=0.1):
10791196
10801197
'''
10811198

1082-
r2 = self._copy()
1199+
r2 = self.copy()
10831200
r2.name = 'P/' + self.name
10841201

10851202
for i in range(self.n):

0 commit comments

Comments
 (0)
0