8000 added fdyn and printdyn methods · navrobot/robotics-toolbox-python@200524b · GitHub
[go: up one dir, main page]
Skip to content

Commit 200524b

Browse files
petercorkepetercorke
committed
added fdyn and printdyn methods
minor tweaks elsewhere to suit
1 parent 5469413 commit 200524b

File tree

2 files changed

+255
-28
lines changed

2 files changed

+255
-28
lines changed

‎roboticstoolbox/robot/DHLink.py

Lines changed: 23 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -342,28 +342,30 @@ def _copy(self):
342342

343343
return copy.copy(self)
344344

345-
def dyn(self):
345+
def dyn(self, indent=0):
346346
"""
347347
Show inertial properties of link
348348
349349
s = dyn() returns a string representation the inertial properties of
350350
the link object in a multi-line format. The properties shown are mass,
351351
centre of mass, inertia, friction, gear ratio and motor properties.
352352
353+
:param indent: indent each line by this many spaces
354+
:type indent: int
353355
:return s: The string representation of the link dynamics
354356
:rtype s: string
355357
"""
356358

357-
s = "m = {:.2g} \n" \
358-
"r = {:.2g} {:.2g} {:.2g} \n" \
359-
" | {:6.2g} {:6.2g} {:6.2g} | \n" \
360-
"I = | {:6.2g} {:6.2g} {:6.2g} | \n" \
361-
" | {:6.2g} {:6.2g} {:6.2g} | \n" \
362-
"Jm = {:.2g} \n" \
363-
"B = {:.2g} \n" \
364-
"Tc = {:.2g}(+) {:.2g}(-) \n" \
365-
"G = {:.2g} \n" \
366-
"qlim = {:.2g} to {:.2g}".format(
359+
s = "m = {:8.2g} \n" \
360+
"r = {:8.2g} {:8.2g} {:8.2g} \n" \
361+
" | {:8.2g} {:8.2g} {:8.2g} | \n" \
362+
"I = | {:8.2g} {:8.2g} {:8.2g} | \n" \
363+
" | {:8.2g} {:8.2g} {:8.2g} | \n" \
364+
"Jm = {:8.2g} \n" \
365+
"B = {:8.2g} \n" \
366+
"Tc = {:8.2g}(+) {:8.2g}(-) \n" \
367+
"G = {:8.2g} \n" \
368+
"qlim = {:8.2g} to {:8.2g}".format(
367369
self.m,
368370
self.r[0, 0], self.r[1, 0], self.r[2, 0],
369371
self.I[0, 0], self.I[0, 1], self.I[0, 2],
@@ -376,6 +378,12 @@ def dyn(self):
376378
self.qlim[0], self.qlim[1]
377379
)
378380

381+
if indent >0:
382+
# insert indentations into the string
383+
# TODO there is probably a tidier way to integrate this step with above
384+
sp = ' ' * indent
385+
s = sp + s.replace('\n', '\n' + sp)
386+
379387
return s
380388

381389
def A(self, q):
@@ -483,14 +491,14 @@ def isprismatic(self):
483491

484492
def nofriction(self, coulomb=True, viscous=False):
485493
"""
486-
l2 = nofriction(coulomb, viscous) copies the link and returns a link
494+
``l2 = nofriction(coulomb, viscous)`` copies the link and returns a link
487495
with the same parameters except, the Coulomb and/or viscous friction
488496
parameter to zero.
489497
490-
l2 = nofriction() as above except the the Coulomb parameter is set to
498+
``l2 = nofriction()`` as above except the the Coulomb parameter is set to
491499
zero.
492500
493-
:param coulomb: if True, will set the coulomb friction to 0
501+
:param coulomb: if True, will set the Coulomb friction to 0
494502
:type coulomb: bool
495503
:param viscous: if True, will set the viscous friction to 0
496504
:type viscous: bool
@@ -509,7 +517,7 @@ def nofriction(self, coulomb=True, viscous=False):
509517

510518
def friction(self, qd):
511519
"""
512-
tau = friction(qd) Calculates the joint friction force/torque (n)
520+
``tau = friction(qd)`` Calculates the joint friction force/torque (n)
513521
for joint velocity qd (n). The friction model includes:
514522
515523
- Viscous friction which is a linear function of velocity.

‎roboticstoolbox/robot/Dynamics.py

Lines changed: 232 additions & 13 deletions
1241
Original file line numberDiff line numberDiff line change
@@ -10,15 +10,231 @@
1010
1111
:todo: perhaps these should be abstract properties, methods of this calss
1212
"""
13-
13+
import copy
14+
from collections import namedtuple
1415
import numpy as np
1516
import roboticstoolbox as rp
1617
from spatialmath.base.argcheck import \
17-
getvector, verifymatrix
18-
18+
getvector, verifymatrix, isscalar
19+
from scipy import integrate, interpolate
1920

2021
class Dynamics:
2122

23+
def printdyn(self):
24+
"""
25+
Print dynamic parameters
26+
27+
Display the kinematic and dynamic parameters to the console in
28+
reable format
29+
"""
30+
for j, link in enumerate(self.links):
31+
print("\nLink {:d}::".format(j), link)
32+
print(link.dyn(indent=2))
33+
34+
35+
def fdyn(self, T, q0, torqfun=None, targs=None, qd0=None,
36+
solver='RK45', sargs=None, dt=None, progress=False):
37+
"""
38+
Integrate forward dynamics
39+
40+
:param T: integration time
41+
:type T: float
42+
:param q0: initial joint coordinates
43+
:type q0: array_like
44+
:param qd0: initial joint velocities, assumed zero if not given
45+
:type qd0: array_like
46+
:param torqfun: a function that computes torque as a function of time
47+
and/or state
48+
:type torqfun: callable
49+
:param targs: argumments passed to ``torqfun``
50+
:type targs: dict
51+
:type solver: name of scipy solver to use, RK45 is the default
52+
:param solver: str
53+
:type sargs: arguments passed to the solver
54+
:param sargs: dict
55+
:type dt: time step for results
56+
:param dt: float
57+
:param progress: show progress bar, default False
58+
:type progress: bool
59+
60+
:return: robot trajectory
61+
:rtype: namedtuple
62+
63+
- ``tg = R.fdyn(T, q)`` integrates the dynamics of the robot with zero
64+
input torques over the time interval 0 to ``T`` and returns the
65+
trajectory as a namedtuple with elements:
66+
67+
- ``t`` the time vector (M,)
68+
- ``q`` the joint coordinates (M,n)
69+
- ``qd`` the joint velocities (M,n)
70+
71+
- ``tg = R.fdyn(T, q, torqfun)`` as above but the torque applied to the
72+
joints is given by the provided function::
73+
74+
tau = function(robot, t, q, qd, **args)
75+
76+
where the inputs are:
77+
78+
- the robot object
79+
- current time
80+
- current joint coordinates (n,)
81+
- current joint velocity (n,)
82+
- args, optional keyword arguments can be specified, these are
83+
passed in from the ``targs`` kewyword argument.
84+
85+
The function must return a Numpy array (n,) of joint forces/torques.
86+
87+
Examples:
88+
89+
#. to apply zero joint torque to the robot without Coulomb
90+
friction::
91+
92+
def myfunc(robot, t, q, qd):
93+
return np.zeros((robot.n,))
94+
95+
tg = robot.nofriction().fdyn(5, q0, myfunc)
96+
97+
plt.figure()
98+
plt.plot(tg.t, tg.q)
99+
plt.show()
100+
101+
We could also use a lambda function::
102+
103+
tg = robot.nofriction().fdyn(5, q0, lambda r, t, q, qd: np.zeros((r.n,)))
104+
105+
#. the robot is controlled by a PD controller. We first define a
106+
function to compute the control which has additional parameters for
107+
the setpoint and control gains (qstar, P, D)::
108+
109+
def myfunc(robot, t, q, qd, qstar, P, D):
110+
return (qstar - q) * P + qd * D # P, D are (6,)
111+
112+
targs = {'qstar': VALUE, 'P': VALUE, 'D': VALUE}
113+
tg = robot.fdyn(10, q0, myfunc, targs=targs) )
114+
115+
Many integrators have variable step length which is problematic if we
116+
want to animate the result. If ``dt`` is specified then the solver
117+
results are interpolated in time steps of ``dt``.
118+
119+
:notes:
120+
121+
- This function performs poorly with non-linear joint friction, such as
122+
Coulomb friction. The R.nofriction() method can be used to set this
123+
friction to zero.
124+
- If the function is not specified then zero force/torque is
125+
applied to the manipulator joints.
126+
- Interpolation is performed using `ScipY integrate.ode <https://docs.scipy.org/doc/scipy/reference/generated/scipy.integrate.ode.html>`
127+
- The SciPy RK45 integrator is used by default
128+
- Interpolation is performed using `SciPy interp1 <https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.interp1d.html>`
129+
130+
:seealso: :func:`DHRobot.accel`, :func:`DHRobot.nofriction`, :func:`DHRobot.rne`.
131+
"""
132+
133+
n = self.n
134+
135+
if not isscalar(T):
136+
raise ValueError('T must be a scalar')
137+
q0 = getvector(q0, n)
138+
if qd0 is None:
139+
qd0 = np.zeros((n,))
140+
else:
141+
qd0 = getvector(qd0, n)
142+
if torqfun is not None:
143+
if not callable(torqfun):
144+
raise ValueError('torque function must be callable')
145+
if sargs is None:
146+
sargs = {}
147+
if targs is None:
148+
targs = {}
149+
150+
# concatenate q and qd into the initial state vector
151+
x0 = np.r_[q0, qd0]
152+
153+
scipy_integrator = integrate.__dict__[solver] # get user specified integrator
154+
155+
integrator = scipy_integrator(
156+
lambda t, y: self._fdyn(t, y, torqfun, targs),
157+
t0=0.0, y0=x0, t_bound=T, **sargs
158+
)
159+
160+
# initialize list of time and states
161+
tlist = [0]
162+
xlist = [np.r_[q0, qd0]]
163+
164+
if progress:
165+
_printProgressBar(0, prefix='Progress:', suffix='complete', length=60)
166+
167+
while integrator.status == 'running':
168+
169+
# step the integrator, calls _fdyn multiple times
170+
integrator.step()
171+
172+
if integrator.status == 'failed':
173+
raise RuntimeError('integration completed with failed status ')
174+
175+
# stash the results
176+
tlist.append(integrator.t)
177+
xlist.append(integrator.y)
178+
179+
# update the progress bar
180+
if progress:
181+
_printProgressBar(integrator.t / T, prefix='Progress:', suffix='complete', length=60)
182+
183+
# cleanup the progress bar
184+
if progress:
185+
print('\r' + ' '* 90 + '\r')
186+
187+
tarray = np.array(tlist)
188+
xarray = np.array(xlist)
189+
190+
if dt is not None:
191+
# interpolate data to equal time steps of dt
192+
interp = interpolate.interp1d(tarray, xarray, axis=0)
193+
194+
tnew = np.arange(0, T, dt)
195+
xnew = interp(tnew)
196+
return namedtuple('fdyn', 't q qd')(tnew, xnew[:, :n], xnew[:, n:])
197+
else:
198+
return namedtuple('fdyn', 't q qd')(tarray, xarray[:, :n], xarray[:, n:])
199+
200+
def _fdyn(self, t, x, torqfun, targs):
201+
"""
202+
Private function called by fdyn
203+
204+
:param t: current time
205+
:type t: float
206+
:param x: current state [q, qd]
207+
:type x: numpy array (2n,)
208+
:param torqfun: a function that computes torque as a function of time
209+
and/or state
210+
:type torqfun: callable
211+
:param targs: argumments passed to ``torqfun``
212+
:type targs: dict
213+
214+
:return: derivative of current state [qd, qdd]
215+
:rtype: numpy array (2n,)
216+
217+
Called by ``fdyn`` to evaluate the robot velocity and acceleration for
218+
forward dynamics.
219+
"""
220+
n = self.n
221+
222+
q = x[0:n]
223+
qd = x[n:]
224+
225+
# evaluate the torque function if one is given
226+
if torqfun is None:
227+
tau = np.zeros((n,))
228+
else:
229+
tau = torqfun(self, t, q, qd, **targs)
230+
if len(tau) != n or not all(np.isreal(tau)):
231+
raise RuntimeError('torque function must return vector with N real elements')
232+
233+
qdd = self.accel(q, qd, tau)
234+
235+
return np.r_[qd, qdd]
236+
237+
22238
def accel(self, q, qd, torque, ):
23239
"""
24240
Compute acceleration due to applied torque
@@ -113,18 +329,14 @@ def nofriction(self, coulomb=True, viscous=False):
113329
114330
"""
115331

116-
L = []
332+
# shallow copy the robot object
333+
nf = copy.copy(self)
334+
nf.name = 'NF/' + self.name
117335

118-
for i in range(self.n):
119-
L.append(self.links[i].nofriction(coulomb, viscous))
336+
# add the modified links (copies)
337+
nf._links = [link.nofriction(coulomb, viscous) for link in self.links]
120338

121-
return rp.SerialLink(
122-
L,
123-
name='NF' + self.name,
124-
manufacturer=self.manuf,
125-
base=self.base,
126-
tool=self.tool,
127-
gravity=self.gravity)
339+
return nf
128340

129341
def pay(self, W, q=None, J=None, frame=1):
130342
"""
@@ -866,3 +1078,10 @@ def perterb(self, p=0.1):
8661078
r2.links[i].I = r2.links[i].I * s # noqa
8671079

8681080
return r2
1081+
1082+
def _printProgressBar (fraction, prefix='', suffix='', decimals=1, length=50, fill = '█', printEnd = "\r"):
1083+
1084+
percent = ("{0:." + str(decimals) + "f}").format(fraction * 100)
1085+
filledLength = int(length * fraction)
1086+
bar = fill * filledLength + '-' * (length - filledLength)
1087+
print(f'\r{prefix} |{bar}| {percent}% {suffix}', end = printEnd)

0 commit comments

Comments
 (0)
0