8000 Merge branch 'master' of https://github.com/petercorke/robotics-toolb… · navrobot/robotics-toolbox-python@01658eb · GitHub
[go: up one dir, main page]
Skip to content

Commit 01658eb

Browse files
micah-huthmicah-huth
committed
Merge branch 'master' of https://github.com/petercorke/robotics-toolbox-python into micah-dev
2 parents 06c363f + f6f1be4 commit 01658eb

File tree

7 files changed

+511
-15
lines changed

7 files changed

+511
-15
lines changed

‎roboticstoolbox/jtraj.py

Lines changed: 128 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,128 @@
1+
import numpy as np
2+
import spatialmath.base.argcheck as arg
3+
from collections import namedtuple
4+
5+
def jtraj(q0, q1, tv, qd0=None, qd1=None, vel=False, accel=False):
6+
"""
7+
JTRAJ Compute a joint space trajectory
8+
9+
:param q0: initial coordinate
10+
:type q0: array_like
11+
:param q1: final coordinate
12+
:type q1: array_like
13+
:param tv: time vector or number of steps
14+
:type tv: array_like or int
15+
:param vel: output velocity trajectory, defaults to False
16+
:type vel: bool, optional
17+
:param accel: output acceleration trajectory, defaults to False
18+
:type accel: bool, optional
19+
:param qd0: initial velocity, defaults to zero
20+
:type qd0: array_like, optional
21+
:param qd1: final velocity, defaults to zero
22+
:type qd1: array_like, optional
23+
:return: trajectory of coordinates and optionally velocity and acceleration
24+
:rtype: np.ndarray, or namedtuple of np.ndarray
25+
26+
27+
``Q = JTRAJ(Q0, QF, M)`` is a joint space trajectory ``Q`` (MxN) where the joint
28+
coordinates vary from ``Q0`` (1xN) to ``QF`` (1xN). A quintic (5th order) polynomial is used
29+
with default zero boundary conditions for velocity and acceleration.
30+
Time is assumed to vary from 0 to 1 in ``M`` steps.
31+
32+
``Q = JTRAJ(Q0, QF, M, QD0, QDF)`` as above but also specifies
33+
initial ``QD0`` (1xN) and final ``QDF`` (1xN) joint velocity for the trajectory.
34+
35+
``Q = JTRAJ(Q0, QF, T)`` as above but the number of steps in the
36+
trajectory is defined by the length of the time vector ``T`` (Mx1).
37+
38+
``Q = JTRAJ(Q0, QF, T, QD0, QDF)`` as 67E6 above but specifies initial and
39+
final joint velocity for the trajectory and a time vector.
40+
41+
The output ``Q`` is an MxN numpy array with one row per time step.
42+
43+
Joint velocity and acceleration can be optionally returned by setting
44+
``vel`` or ``accel`` to True. In this case the output is a named tuple
45+
with elements `q`, `qd` and `qdd`. The shape of the velocity and acceleration
46+
arrays is the same as for ``Q``.
47+
48+
Notes:
49+
50+
- When a time vector is provided the velocity and acceleration outputs
51+
are scaled assumign that the time vector starts at zero and increases
52+
linearly.
53+
54+
See also QPLOT, CTRAJ, SerialLink.jtraj.
55+
56+
Copyright (C) 1993-2017, by Peter I. Corke
57+
58+
"""
59+
if isinstance(tv, int):
60+
tscal = 1
61+
t = np.linspace(0, 1, tv) # normalized time from 0 -> 1
62+
else:
63+
tscal = max(tv)
64+
t = tv.flatten() / tscal
65+
66+
q0 = arg.getvector(q0)
67+
q1 = arg.getvector(q1)
68+
assert len(q0) == len(q1), 'q0 and q1 must be same size'
69+
70+
if qd0 is None:
71+
qd0 = np.zeros(q0.shape)
72+
else:
73+
qd0 = arg.getvector(qd0)
74+
assert len(qd0) == len(q0), 'qd0 has wrong size'
75+
if qd1 is None:
76+
qd1 = np.zeros(q0.shape)
77+
else:
78+
qd0 = arg.getvector(qd0)
79+
assert len(qd1) == len(q0), 'qd1 has wrong size'
80+
81+
# compute the polynomial coefficients
82+
A = 6 * (q1 - q0) - 3 * (qd1 + qd0) * tscal
83+
B = -15 * (q1 - q0) + (8 * qd0 + 7 * qd1) * tscal
84+
C = 10 * (q1 - q0) - (6 * qd0 + 4 * qd1) * tscal
85+
E = qd0 * tscal # as the t vector has been normalized
86+
F = q0
87+
88+
n = len(q0)
89+
90+
tt = np.array([t**5, t**4, t**3, t**2, t, np.ones(t.shape)]).T
91+
c = np.array([A, B, C, np.zeros(A.shape), E, F])
92+
93+
qt = tt @ c
94+
95+
if not vel and not accel:
96+
return qt
97+
98+
if vel and not accel:
99+
out = namedtuple('jtraj', 'q qd')
100+
elif not vel and accel:
101+
out = namedtuple('jtraj', 'q qdd')
102+
else:
103+
out = namedtuple('jtraj', 'q qd qdd')
104+
105+
out.q = qt
106+
if vel:
107+
# compute optional velocity
108+
c = np.array([np.zeros(A.shape), 5 * A, 4 * B, 3 * C, np.zeros(A.shape), E])
109+
qdt = tt @ c / tscal
110+
out.qd = qdt
111+
112+
# compute optional acceleration
113+
if accel:
114+
c = np.array([np.zeros(A.shape), np.zeros(A.shape), 20 * A, 12 * B, 6 * C, np.zeros(A.shape)])
115+
qddt = tt @ c / tscal**2
116+
out.qdd = qddt
117+
118+
return out
119+
120+
if __name__ == "__main__":
121+
import matplotlib.pyplot as plt
122+
123+
out = jtraj([0,1], [2, -1], 20)
124+
print(out)
125+
126+
out = jtraj([0,1], [2, -1], 20, vel=True, accel=True)
127+
print(out)
128+

‎roboticstoolbox/mstraj.py

Lines changed: 264 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,264 @@
1+
import numpy as np
2+
import math
3+
4+
import jtraj
5+
from collections import namedtuple
6+
7+
8+
def mstraj(viapoints, dt, tacc, qdmax=None, tsegment=None, q0=None, qd0=None, qdf=None, extra=False, verbose=False):
9+
10+
"""
11+
MSTRAJ Multi-segment multi-axis trajectory
12+
13+
:param viapoints: A set of viapoints, one per row
14+
:type viapoints: numpy.ndarray
15+
:param dt: time step
16+
:type dt: float (seconds)
17+
:param tacc: acceleration time (seconds)
18+
:type tacc: float
19+
:param qdmax: maximum joint speed, defaults to None
20+
:type qdmax: array_like or float, optional
21+
:param tsegment: maximum time of each motion segment (seconds), defaults to None
22+
:type tsegment: array_like, optional
23+
:param q0: initial joint coordinates, defaults to first row of viapoints
24+
:type q0: array_like, optional
25+
:param qd0: inital joint velocity, defaults to zero
26+
:type qd0: array_like, optional
27+
:param qdf: final joint velocity, defaults to zero
28+
:type qdf: array_like, optional
29+
:param extra: return extra information, defaults to False
30+
:type extra: bool, optional
31+
:param verbose: print debug information, defaults to False
32+
:type verbose: bool, optional
33+
:return: trajectory or trajectory plus extra info
34+
:rtype: np.ndarray,
35+
36+
``TRAJ = MSTRAJ(WP, QDMAX, TSEG, Q0, DT, TACC)`` is a trajectory
37+
(KxN) for N axes moving simultaneously through M segment. Each segment
38+
is linear motion and polynomial blends connect the viapoints. The axes
39+
start at ``Q0`` (1xN) if given and pass through the via points defined by the rows of
40+
the matrix WP (MxN), and finish at the point defined by the last row of WP.
41+
The trajectory matrix has one row per time step, and one column per
42+
axis. The number of steps in the trajectory K is a function of the
43+
number of via points and the time or velocity limits that apply.
44+
45+
- WP (MxN) is a matrix of via points, 1 row per via point, one column
46+
per axis. The last via point is the destination.
47+
- QDMAX (1xN) are axis speed limits which cannot be exceeded,
48+
- TSEG (1xM) are the durations for each of the K viapoints
49+
- Q0 (1xN) are the initial axis coordinates
50+
- DT is the time step
51+
- TACC (1x1) is the acceleration time used for all segment transitions
52+
- TACC (1xM) is the acceleration time per segment, TACC(i) is the acceleration
53+
time for the transition from segment i to segment i+1. TACC(1) is also
54+
the acceleration time at the start of segment 1.
55+
56+
TRAJ = MSTRAJ(WP, QDMAX, TSEG, [], DT, TACC, OPTIONS) as above but the
57+
initial coordinates are taken from the first row of WP.
58+
59+
TRAJ = MSTRAJ(WP, QDMAX, Q0, DT, TACC, QD0, QDF, OPTIONS) as above
60+
but additionally specifies the initial and final axis velocities (1xN).
61+
62+
63+
Notes::
64+
- Only one of QDMAX or TSEG can be specified, the other is set to [].
65+
- If no output arguments are specified the trajectory is plotted.
66+
- The path length K is a function of the number of via points, Q0, DT
67+
and TACC.
68+
- The final via point P(end,:) is the destination.
69+
- The motion has M viapoints from Q0 to P(1,:) to P(2,:) ... to P(end,:).
70+
- All axes reach their via points at the same time.
71+
- Can be used to create joint space trajectories where each axis is a joint
72+
coordinate.
73+
- Can be used to create Cartesian trajectories where the "axes"
74+
correspond to translation and orientation in RPY or Euler angle form.
75+
- If qdmax is a scalar then all axes are assumed to have the same
76+
maximum speed.
77+
78+
See also MTRAJ, LSPB, CTRAJ.
79+
80+
Copyright (C) 1993-2017, by Peter I. Corke
81+
"""
82+
83+
if q0 is None:
84+
q0 = viapoints[0,:]
85+
viapoints = viapoints[1:,:]
86+
else:
87+
assert viapoints.shape[1] == len(q0), 'WP and Q0 must have same number of columns'
88+
89+
ns, nj = viapoints.shape
90+
Tacc = tacc
91+
92+
assert not (qdmax is not None and tsegment is not None), 'cannot specify both qdmax and tsegment'
93+
if qdmax is None:
94+
assert tsegment is not None, 'tsegment must be given if qdmax is not'
95+
assert len(tsegment) == ns, 'Length of TSEG does not match number of viapoints'
96+
if tsegment is None:
97+
assert qdmax is not None, 'qdmax must be given if tsegment is not'
98+
if isinstance(qdmax, (int, float)):
99+
# if qdmax is a scalar assume all axes have the same speed
100+
qdmax = np.tile(qdmax, (nj,))
101+
else:
102+
assert len(qdmax) == nj, 'Length of QDMAX does not match number of axes'
103+
104+
if isinstance(Tacc, (int, float)):
105+
Tacc = np.tile(Tacc, (ns,))
106+
else:
107+
assert len(Tacc) == ns, 'Tacc is wrong size'
108+
if qd0 is None:
109+
qd0 = np.zeros((nj,))
110+
else:
111+
assert len(qd0) == len(q0), 'qd0 is wrong size'
112+
if qdf is None:
113+
qdf = np.zeros((nj,))
114+
else:
115+
assert len(qdf) == len(q0), 'qdf is wrong size'
116+
117+
# set the initial conditions
118+
q_prev = q0;
119+
qd_prev = qd0;
120+
121+
clock = 0 # keep track of time
122+
arrive = np.zeros((ns,)) # record planned time of arrival at via points
123+
tg = np.zeros((0,nj))
124+
infolist = []
125+
info = namedtuple('mstraj_info', 'slowest segtime axtime clock')
126+
127+
for seg in range(0, ns):
128+
if verbose:
129+
print('------------------- segment %d\n' % (seg,))
130+
131+
# set the blend time, just half an interval for the first segment
132+
133+
tacc = Tacc[seg]
134+
135+
tacc = math.ceil(tacc / dt) * dt
136+
tacc2 = math.ceil(tacc / 2 / dt) * dt
137+
if seg == 0:
138+
taccx = tacc2
139+
else:
140+
taccx = tacc
141+
142+
# estimate travel time
143+
# could better estimate distance travelled during the blend
144+
q_next = viapoints[seg,:] # current target
145+
dq = q_next - q_prev # total distance to move this segment
146+
147+
## probably should iterate over the next section to get qb right...
148+
# while 1
149+
# qd_next = (qnextnext - qnext)
150+
# tb = abs(qd_next - qd) ./ qddmax;
151+
# qb = f(tb, max acceleration)
152+
# dq = q_next - q_prev - qb
153+
# tl = abs(dq) ./ qdmax;
154+
155+
if qdmax is not None:
156+
# qdmax is specified, compute slowest axis
157+
158+
qb = taccx * qdmax / 2 # distance moved during blend
159+
tb = taccx
160+
161+
# convert to time
162+
tl = abs(dq) / qdmax
163+
#tl = abs(dq - qb) / qdmax
164+
tl = np.ceil(tl / dt) * dt
165+
166+
# find the total time and slowest axis
167+
tt = tb + tl
168+
slowest = np.argmax(tt)
169+
tseg = tt[slowest]
170+
171+
infolist.append(info(slowest, tseg, tt, clock))
172+
173+
# best if there is some linear motion component
174+
if tseg <= 2*tacc:
175+
tseg = 2 * tacc
176+
177+
elif tsegment is not None:
178+
# segment time specified, use that
179+
tseg = tsegment[seg]
180+
slowest = math.nan
181+
182+
# log the planned arrival time
183+
arrive[seg] = clock + tseg
184+
if seg > 0:
185+
arrive[seg] += tacc2
186+
187+
if verbose:
188+
print('seg %d, slowest axis %d, time required %.4g\n' % (seg, slowest, tseg))
189+
190+
## create the trajectories for this segment
191+
192+
# linear velocity from qprev to qnext
193+
qd = dq / tseg
194+
195+
# add the blend polynomial
196+
qb = jtraj.jtraj(q0, q_prev + tacc2 * qd, np.arange(0, taccx, dt), qd0=qd_prev, qd1=qd)
197+
tg = np.vstack([tg, qb[1:,:]])
198+
199+
clock = clock + taccx # update the clock
200+
201+
# add the linear part, from tacc/2+dt to tseg-tacc/2
202+
for t in np.arange(tacc2 + dt, tseg - tacc2, dt):
203+
s = t / tseg
204+
q0 = (1 - s) * q_prev + s * q_next # linear step
205+
tg = np.vstack([tg, q0])
206+
clock += dt
207+
208+
q_prev = q_next # next target becomes previous target
209+
qd_prev = qd
210+
211+
# add the final blend
212+
qb = jtraj.jtraj(q0, q_next, np.arange(0, tacc2, dt), qd0=qd_prev, qd1=qdf)
213+
tg = np.vstack([tg, qb[1:,:]])
214+
215+
print(info)
216+
217+
infolist.append(info(None, tseg, None, clock))
218+
219+
if extra:
220+
out = namedtuple('mstraj', 't q arrive info')
221+
out.t = dt * np.arange(0, tg.shape[0])
222+
out.q = tg
223+
out.arrive = arrive
224+
out.info = infolist
225+
out.viapoints = viapoints
226+
return out
227+
else:
228+
return tg
229+
230+
231+
#return (TG, t, info)
232+
233+
return tg
234+
235+
if __name__ == "__main__":
236+
import matplotlib.pyplot as plt
237+
238+
path = np.array([
239+
[10, 10],
240+
[10, 60],
241+
[80, 80],
242+
[50, 10]
243+
])
244+
245+
out = mstraj(path, dt=0.1, tacc=5, qdmax=2.5, extra=True)
246+
print(out.q)
247+
248+
plt.figure()
249+
plt.plot(out.t, out.q)
250+
plt.grid(True)
251+
plt.xlabel('time')
252+
plt.legend(('$q_0$', '$q_1$'))
253+
plt.plot(out.arrive, out.viapoints, 'bo')
254+
255+
plt.figure()
256+
plt.plot(out.q[:,0], out.q[:,1])
257+
plt.xlabel('X')
258+
plt.ylabel('Y')
259+
plt.grid(True)
260+
261+
262+
263+
#plt.xaxis(t(1), t(end))
264+

0 commit comments

Comments
 (0)
0