8000 Added trajectory functions for plotting · navrobot/robotics-toolbox-python@ef1a595 · GitHub
[go: up one dir, main page]
Skip to content

Commit ef1a595

Browse files
Samuel-DrewSamuel-Drew
committed
Added trajectory functions for plotting
1 parent 69638a2 commit ef1a595

File tree

3 files changed

+232
-82
lines changed

3 files changed

+232
-82
lines changed

‎roboticstoolbox/robot/serial_link.py

Lines changed: 30 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -63,22 +63,41 @@ def length(self):
6363
"""
6464
return len(self.links)
6565

66-
def fkine(self, stance, unit='rad'):
66+
def fkine(self, jointconfig, unit='rad', alltout=False):
6767
"""
6868
Calculates forward kinematics for a list of joint angles.
69-
:param stance: stance is list of joint angles.
70-
:param unit: unit of input angles.
69+
:param jointconfig: stance is list of joint angles.
70+
:param unit: unit of input angles. 'rad' or 'deg'.
71+
:param alltout: request intermediate transformations
7172
:return: homogeneous transformation matrix.
7273
"""
73-
if type(stance) is np.ndarray:
74-
stance = stance
74+
if type(jointconfig) == list:
75+
jointconfig = argcheck.getvector(jointconfig)
7576
if unit == 'deg':
76-
stance = stance * pi / 180
77-
t = SE3(self.base)
78-
for i in range(self.length):
79-
t = t * self.links[i].A(stance[i])
80-
t = t * SE3(self.tool)
81-
return t
77+
jointconfig = jointconfig * pi / 180
78+
if alltout:
79+
allt = list(range(0, self.length))
80+
if jointconfig.size == self.length:
81+
t = SE3(self.base)
82+
for i in range(self.length):
83+
t = t * self.links[i].A(jointconfig[i])
84+
if alltout:
85+
allt[i] = t
86+
t = t * SE3(self.tool)
87+
else:
88+
assert jointconfig.shape[1] == self.length, "joinconfig must have {self.length} columns"
89+
t = list(range(0, jointconfig.shape[0]))
90+
for k in range(jointconfig.shape[0]):
91+
qk = jointconfig[k, :]
92+
tt = SE3(self.base)
93+
for i in range(self.length):
94+
tt = tt * self.links[i].A(qk[i])
95+
t[k] = tt * SE3(self.tool)
96+
97+
if alltout:
98+
return t, allt
99+
else:
100+
return t
82101

83102
def ikine(self, T, q0=None, unit='rad'):
84103
"""

‎roboticstoolbox/robot/trajectory.py

Lines changed: 75 additions & 71 deletions
Original file line numberDiff line numberDiff line change
@@ -7,11 +7,13 @@
77
the authors is made.
88
99
@author: Luis Fernando Lara Tobar and Peter Corke
10+
11+
Edited 3/06 Samuel Drew
1012
"""
1113

12-
from numpy import *
13-
from utility import *
14-
import transform as T
14+
import numpy as np
9E81
15+
from spatialmath.base.argcheck import *
16+
1517

1618
def jtraj(q0, q1, tv, qd0=None, qd1=None):
1719
"""
@@ -44,82 +46,84 @@ def jtraj(q0, q1, tv, qd0=None, qd1=None):
4446
4547
"""
4648

47-
if isinstance(tv,(int,int32,float,float64)):
48-
tscal = float(1)
49-
t = mat(range(0,tv)).T/(tv-1.) # Normalized time from 0 -> 1
49+
if isscalar(tv):
50+
tscal = 1
51+
t = np.array([range(0, tv)]).T/(tv-1) # Normalized time from 0 -> 1
5052
else:
51-
tv = arg2array(tv);
52-
tscal = float(max(tv))
53-
t = mat(tv).T / tscal
53+
tv = getvector(tv)
54+
tscal = max(tv)
55+
t = tv[np.newaxis].T / tscal
5456

55-
q0 = arg2array(q0)
56-
q1 = arg2array(q1)
57+
q0 = getvector(q0)
58+
q1 = getvector(q1)
5759

5860
if qd0 == None:
59-
qd0 = zeros((shape(q0)))
61+
qd0 = np.zeros(q0.shape)
6062
else:
61-
qd0 = arg2array(qd0);
63+
qd0 = getvector(qd0);
6264
if qd1 == None:
63-
qd1 = zeros((shape(q1)))
65+
qd1 = np.zeros(q1.shape)
6466
else:
65-
qd1 = arg2array(qd1)
66-
67-
print(qd0)
68-
print(qd1)
67+
qd1 = getvector(qd1)
6968

7069
# compute the polynomial coefficients
71-
A = 6*(q1 - q0) - 3*(qd1 + qd0)*tscal
72-
B = -15*(q1 - q0) + (8*qd0 + 7*qd1)*tscal
73-
C = 10*(q1 - q0) - (6*qd0 + 4*qd1)*tscal
74-
E = qd0*tscal # as the t vector has been normalized
75-
F = q0
76-
77-
tt = concatenate((power(t,5),power(t,4),power(t,3),power(t,2),t,ones(shape(t))),1)
78-
c = vstack((A, B, C, zeros(shape(A)), E, F))
79-
qt = tt * c
80-
81-
# compute velocity
82-
c = vstack((zeros(shape(A)),5*A,4*B,3*C,zeros(shape(A)),E))
83-
qdt = tt * c / tscal
84-
85-
86-
# compute acceleration
87-
c = vstack((zeros(shape(A)),zeros(shape(A)),20*A,12*B,6*C,zeros(shape(A))))
88-
qddt = tt * c / (tscal**2)
89-
90-
return qt,qdt,qddt
91-
92< 10000 span class="diff-text-marker">-
93-
def ctraj(t0, t1, r):
94-
"""
95-
Compute a Cartesian trajectory between poses C{t0} and C{t1}.
96-
The number of points is the length of the path distance vector C{r}.
97-
Each element of C{r} gives the distance along the path, and the
98-
must be in the range [0 1].
99-
100-
If {r} is a scalar it is taken as the number of points, and the points
101-
are equally spaced between C{t0} and C{t1}.
102-
103-
The trajectory is a list of transform matrices.
104-
105-
@type t0: homogeneous transform
106-
@param t0: initial pose
107-
@rtype: list of M{4x4} matrices
108-
@return: Cartesian trajectory
109-
@see: L{trinterp}, L{jtraj}
110-
"""
111-
112-
if isinstance(r,(int,int32,float,float64)):
113-
i = mat(range(1,r+1))
114-
r = (i-1.)/(r-1)
115-
else:
116-
r = arg2array(r);
117-
118-
if any(r>1) or any(r<0):
119-
raise 'path position values (R) must 0<=R<=1'
120-
traj = []
121-
for s in r.T:
122-
traj.append( T.trinterp(t0, t1, float(s)) )
123-
return traj
70+
a = 6*(q1 - q0) - 3*(qd1 + qd0)*tscal
71+
b = -15*(q1 - q0) + (8*qd0 + 7*qd1)*tscal
72+
c = 10*(q1 - q0) - (6*qd0 + 4*qd1)*tscal
73+
e = qd0*tscal # as the t vector has been normalized
74+
f = q0
75+
76+
tt = np.concatenate((np.power(t, 5),
77+
np.power(t, 4),
78+
np.power(t, 3),
79+
np.power(t, 2),
80+
t, np.ones(t.shape)),1)
81+
c = np.vstac F987 k((a, b, c, np.zeros(a.shape), e, f))
82+
qt = np.dot(tt, c)
83+
84+
# # compute velocity
85+
# c = vstack((zeros(shape(a)),5*a,4*b,3*c,zeros(shape(a)),e))
86+
# qdt = tt * c / tscal
87+
#
88+
#
89+
# # compute acceleration
90+
# c = vstack((zeros(shape(a)),zeros(shape(a)),20*a,12*b,6*c,zeros(shape(a))))
91+
# qddt = tt * c / (tscal**2)
92+
93+
# return qt,qdt,qddt
94+
return qt
95+
96+
97+
# def ctraj(t0, t1, r):
98+
# """
99+
# Compute a Cartesian trajectory between poses C{t0} and C{t1}.
100+
# The number of points is the length of the path distance vector C{r}.
101+
# Each element of C{r} gives the distance along the path, and the
102+
# must be in the range [0 1].
103+
#
104+
# If {r} is a scalar it is taken as the number of points, and the points
105+
# are equally spaced between C{t0} and C{t1}.
106+
#
107+
# The trajectory is a list of transform matrices.
108+
#
109+
# @type t0: homogeneous transform
110+
# @param t0: initial pose
111+
# @rtype: list of M{4x4} matrices
112+
# @return: Cartesian trajectory
113+
# @see: L{trinterp}, L{jtraj}
114+
# """
115+
#
116+
# if isinstance(r,(int,int32,float,float64)):
117+
# i = mat(range(1,r+1))
118+
# r = (i-1.)/(r-1)
119+
# else:
120+
# r = arg2array(r);
121+
#
122+
# if any(r>1) or any(r<0):
123+
# raise 'path position values (R) must 0<=R<=1'
124+
# traj = []
125+
# for s in r.T:
126+
# traj.append( T.trinterp(t0, t1, float(s)) )
127+
# return traj
124128

125129

‎roboticstoolbox/robot/trajectory_old.py

Lines changed: 127 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,127 @@
1+
"""
2+
Trajectory primitives.
3+
4+
Python implementation by: Luis Fernando Lara Tobar and Peter Corke.
5+
Based on original Robotics Toolbox for Matlab code by Peter Corke.
6+
Permission to use and copy is granted provided that acknowledgement of
7+
the authors is made.
8+
9+
@author: Luis Fernando Lara Tobar and Peter Corke
10+
11+
Edited 3/06 Samuel Drew
12+
"""
13+
14+
from numpy import *
15+
from utility import *
16+
import transform as T
17+
18+
def jtraj(q0, q1, tv, qd0=None, qd1=None):
19+
"""
20+
Compute a joint space trajectory between points C{q0} and C{q1}.
21+
The number of points is the length of the given time vector C{tv}. If
22+
{tv} is a scalar it is taken as the number of points.
23+
24+
A 7th order polynomial is used with default zero boundary conditions for
25+
velocity and acceleration. Non-zero boundary velocities can be
26+
optionally specified as C{qd0} and C{qd1}.
27+
28+
As well as the trajectory, M{q{t}}, its first and second derivatives
29+
M{qd(t)} and M{qdd(t)} are also computed. All three are returned as a tuple.
30+
Each of these is an M{m x n} matrix, with one row per time step, and
31+
one column per joint parameter.
32+
33+
@type q0: m-vector
34+
@param q0: initial state
35+
@type q1: m-vector
36+
@param q1: final state
37+
@type tv: n-vector or scalar
38+
@param tv: time step vector or number of steps
39+
@type qd0: m-vector
40+
@param qd0: initial velocity (default 0)
41+
@type qd1: m-vector
42+
@param qd1: final velocity (default 0)
43+
@rtype: tuple
44+
@return: (q, qd, qdd), a tuple of M{m x n} matrices
45+
@see: L{ctraj}
46+
47+
"""
48+
49+
if isinstance(tv,(int,int32,float,float64)):
50+
tscal = float(1)
51+
t = mat(range(0,tv)).T/(tv-1.) # Normalized time from 0 -> 1
52+
else:
53+
tv = arg2array(tv);
54+
tscal = float(max(tv))
55+
t = mat(tv).T / tscal
56+
57+
q0 = arg2array(q0)
58+
q1 = arg2array(q1)
59+
60+
if qd0 == None:
61+
qd0 = zeros((shape(q0)))
62+
else:
63+
qd0 = arg2array(qd0);
64+
if qd1 == None:
65+
qd1 = zeros((shape(q1)))
66+
else:
67+
qd1 = arg2array(qd1)
68+
69+
print(qd0)
70+
print(qd1)
71+
72+
# compute the polynomial coefficients
73+
A = 6*(q1 - q0) - 3*(qd1 + qd0)*tscal
74+
B = -15*(q1 - q0) + (8*qd0 + 7*qd1)*tscal
75+
C = 10*(q1 - q0) - (6*qd0 + 4*qd1)*tscal
76+
E = qd0*tscal # as the t vector has been normalized
77+
F = q0
78+
79+
tt = concatenate((power(t,5),power(t,4),power(t,3),power(t,2),t,ones(shape(t))),1)
80+
c = vstack((A, B, C, zeros(shape(A)), E, F))
81+
qt = tt * c
82+
83+
# compute velocity
84+
c = vstack((zeros(shape(A)),5*A,4*B,3*C,zeros(shape(A)),E))
85+
qdt = tt * c / tscal
86+
87+
88+
# compute acceleration
89+
c = vstack((zeros(shape(A)),zeros(shape(A)),20*A,12*B,6*C,zeros(shape(A))))
90+
qddt = tt * c / (tscal**2)
91+
92+
return qt,qdt,qddt
93+
94+
95+
def ctraj(t0, t1, r):
96+
"""
97+
Compute a Cartesian trajectory between poses C{t0} and C{t1}.
98+
The number of points is the length of the path distance vector C{r}.
99+
Each element of C{r} gives the distance along the path, and the
100+
must be in the range [0 1].
101+
102+
If {r} is a scalar it is taken as the number of points, and the points
103+
are equally spaced between C{t0} and C{t1}.
104+
105+
The trajectory is a list of transform matrices.
106+
107+
@type t0: homogeneous transform
108+
@param t0: initial pose
109+
@rtype: list of M{4x4} matrices
110+
@return: Cartesian trajectory
111+
@see: L{trinterp}, L{jtraj}
112+
"""
113+
114+
if isinstance(r,(int,int32,float,float64)):
115+
i = mat(range(1,r+1))
116+
r = (i-1.)/(r-1)
117+
else:
118+
r = arg2array(r);
119+
120+
if any(r>1) or any(r<0):
121+
raise 'path position values (R) must 0<=R<=1'
122+
traj = []
123+
for s in r.T:
124+
traj.append( T.trinterp(t0, t1, float(s)) )
125+
return traj
126+
127+

0 commit comments

Comments
 (0)
0