A path is a spatial construct – a locus in space that leads from an initial pose to a final pose. A trajectory is a path with specified timing. For example there is a path from A to B, but there is a trajectory from A to B in $10\ \rm s$ or at $2\ \rm m/ \rm s$.
A path denotes the locus of points in the joint space, or in the operational space, which the manipulator has to follow in the execution of the assigned motion; a path is then a pure geometric description of motion. On the other hand, a trajectory is a path on which a timing law is specified, for instance in terms of velocities and/or accelerations at each point.
In geometry, a locus (plural: loci) (Latin word for "place", "location") is a set of points (commonly, a line, a line segment, a curve or a surface), whose location satisfies or is determined by one or more specified conditions.
The joint space (configuration space) denotes the space in which the $(n \times 1)$ vector of joint variables $$\boldsymbol{\mathrm{q}} = \begin{pmatrix} q_1\\ \vdots \\ q_n \end{pmatrix}$$ is defined; it is $q_i = \theta_i$ for a revolute joint and $q_i = d_i$ for a prismatic joint.
Operational space is the space in which the manipulator task is specified, typically $\mathbb{R}^3$. A position and orientation can then be described by, e.g., a $(6 \times 1)$ vector $\boldsymbol{\mathrm{x}}_e$ of positions $\boldsymbol{\mathrm{p}}_e$ and Euler angles $\boldsymbol{\mathrm{\phi}}_e$ where $$\boldsymbol{\mathrm{x}}_e = \begin{pmatrix} \boldsymbol{\mathrm{p}}_e \\\boldsymbol{\mathrm{\phi}}_e \end{pmatrix}.$$
In [1]:
%%html
<iframe width="560" height="315" src="https://www.youtube.com/embed/pIcxOGo7ieU" frameborder="0" allowfullscreen></iframe>
In [2]:
%%html
<iframe width="560" height="315" src="https://www.youtube.com/embed/0EAKonCFusg" frameborder="0" allowfullscreen></iframe>
In [3]:
%%html
<iframe width="560" height="315" src="https://www.youtube.com/embed/yLNDN0N8DRE" frameborder="0" allowfullscreen></iframe>
In [4]:
%%html
<iframe width="560" height="315" src="https://www.youtube.com/embed/tIIJME8-au8" frameborder="0" allowfullscreen></iframe>
A manipulator motion is typically assigned in the operational space in terms of trajectory parameters such as
If it is desired to plan a trajectory in the joint space, the values of the joint variables have to be determined first from the end-effector position and orientation specified by the user. It is then necessary to resort to an inverse kinematics algorithm
The planning algorithm generates a function $\boldsymbol{\mathrm{q}}(t)$ interpolating the given vectors of joint variables at each point, in respect of the imposed constraints.
In general, a joint space trajectory planning algorithm is required to have the following features:
In point-to-point motion, the manipulator has to move from an initial joint configuration $\boldsymbol{\mathrm{q}}_i$ to a final joint configuration $\boldsymbol{\mathrm{q}}_f$ in a given time $t_f$, that is, $$\boldsymbol{\mathrm{q}}(0) = \boldsymbol{\mathrm{q}}_i$$ and $$\boldsymbol{\mathrm{q}}(t_f) = \boldsymbol{\mathrm{q}}_f.$$
In this case, the actual end-effector path is of no concern.
In the following we consider point-to-point motion of a single joint with position $q(t)$, velocity $\dot q(t)$ and acceleration $\ddot q(t)$.
A joint-space trajectory can be generated using a cubic (third-order) polynomial.
The position $q(t)$ is given by the polynomial $$ q(t) = a_3 t^3 + a_2 t^2 + a_1 t + a_0$$ which results in a parabolic velocity profile $$\dot{q}(t) = 3 a_3 t^2 + 2 a_2 t + a_1$$ and a linear acceleration profile $$\ddot{q}(t) = 6 a_3 t + 2 a_2.$$
The polynomial $q(t)$ has four coeffiecients, $a_3, a_2, a_1, a_0$. It is then possible to impose constraints on the initial position $q(0) = q_i$, the final position $q(t_f) = q_f$, the initial velocity $\dot q(0) = \dot q_i$ and the final velocity $\dot q(tf) = \dot q_f$. The initial and final joint velocity values $q_i$ and $q_f$ are usually set to zero.
In [5]:
qi = 0 % Initital position
qf = pi % Final position
dqi = 0 % Initial velocity
dqf = 0 % Final velocity
t = 2 % Duration of the motion
Determination of a specific trajectory is given by the solution to the following system of equations: \begin{align} a_0 &= q_i \\ a_3 t_f^3 + a_2 t_f^2 + a_1 t_f + a_0 &= q_f \\ a_1 &= \dot{q}_i \\ 3 a_3 t_f^2 + 2 a_2 t_f + a_1 &= \dot{q}_f, \end{align} which can be written in matrix form as: \begin{align} \begin{pmatrix} q_i \\ q_f \\ \dot q_i \\ \dot q_f \end{pmatrix} = \begin{pmatrix} 0 & 0 & 0 & 1 \\ t^3 & t^2 & t & 1 \\ 0 & 0 & 1 & 0 \\ 3t^2 & 2t & 1 & 0 \end{pmatrix} \begin{pmatrix} a_3 \\ a_2 \\ a_1 \\ a_0 \end{pmatrix}. \end{align} Since the matrix is square we can solve for the coefficient vector $(a_3, a_2, a_1, a_0)^\top$ using standard linear algebra methods.
We form the matrix $$ \boldsymbol{\mathrm{A}} = \begin{pmatrix} 0 & 0 & 0 & 1 \\ t^3 & t^2 & t & 1 \\ 0 & 0 & 1 & 0 \\ 3t^2 & 2t & 1 & 0 \end{pmatrix}. $$
In [6]:
A = [0, 0, 0, 1;
t^3, t^2, t, 1;
0, 0, 1, 0;
3 * t^2, 2 * t, 1.0, 0.0]
We then form the vector $$\boldsymbol{\mathrm{b}} = \begin{pmatrix} q_i \\ q_f \\ \dot q_i \\ \dot q_f \end{pmatrix}.$$
In [7]:
b = [qi, qf, dqi, dqf]'
We then solve for the polynomial coeffiecient vector $$\boldsymbol{\mathrm{p}} = \begin{pmatrix} a_3 \\ a_2 \\ a_1 \\ a_0 \end{pmatrix}.$$
In [8]:
p = A \ b
In [9]:
ts = [0:0.01:t];
qs = polyval(p, ts);
The positions $q(t)$ can be plotted:
In [10]:
%plot inline -f 'svg'
plot(ts, qs, 'linewidth',2)
The velocities $\dot q(t)$ can be found by evaluating $$\dot{q}(t) = 3 a_3 t^2 + 2 a_2 t + a_1.$$
In [11]:
dqs = polyval([3 * p(1), 2 * p(2), p(3)],ts);
The velocities $\dot q(t)$ can be plotted:
In [12]:
plot(ts, dqs, 'linewidth',2, 'color',[0,0.5,0])
The accelerations $\ddot q(t)$ can found by evaluating $$\ddot{q}(t) = 6 a_3 t + 2 a_2.$$
In [13]:
ddqs = polyval([6 * p(1), 2 * p(2)],ts);
The accelerations $\ddot q(t)$ can be plotted:
In [14]:
plot(ts, ddqs, 'linewidth', 2, 'color', 'r')
title ("Cubic Trajectory");
xlabel ("time");
ylabel ("acceleration");
We can also create a function that returns the trajectory
In [15]:
function [ts, qs, dqs, ddqs] = cubic_trajectory(qi, qf, dqi, dqf, t)
b = [qi, qf, dqi, dqf]';
A = [0, 0, 0, 1;
t^3, t^2, t, 1;
0, 0, 1, 0;
3 * t^2, 2 * t, 1.0, 0.0];
p = A \ b;
ts = [0:0.01:t];
qs = polyval(p, ts);
dqs = polyval([3 * p(1), 2 * p(2), p(3)],ts);
ddqs = polyval([6 * p(1), 2 * p(2)],ts);
end
In [16]:
[ts, qs, dqs, ddqs] = cubic_trajectory(0, pi, 0, 0, 10);
plot(ts,qs, 'linewidth', 2, ts,dqs, 'linewidth', 2, ts, ddqs, 'linewidth', 2)
If it is desired to assign also the initial and final values of acceleration, six constraints have to be satisfied and then a polynomial of at least fifth-order is needed. The motion timing law for the generic joint is then given by \begin{align} q(t) &= a_5 t^5 + a_4 t^4 + a_3 t^3 + a_2 t^2 + a_1 t + a_0 \\ \dot{q}(t) &= 5 a_5 t^4 + 4 a_4 t^3 + 3 a_3 t^2 + 2 a_2 t + a_1 \\ \ddot{q}(t) &= 20 a_5 t^3 + 12 a_4 t^2 + 6 a_3 t + 2 a_2. \end{align}
This can be written in matrix form as \begin{align} \begin{pmatrix} q_i \\ q_f \\ \dot q_i \\ \dot q_f \\ \ddot q_i \\ \ddot q_f \end{pmatrix} = \begin{pmatrix} 0 & 0 & 0 & 0 & 0 & 1 \\ t^5 & t^4 & t^3 & t^2 & t & 1 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 5t^4 & 4t^3 & 3t^2 & 2t & 1 & 0 \\ 0 & 0 & 0 & 2 & 0 & 0 \\ 20t^3 & 12t^2 & 6t & 2 & 0 & 0 \end{pmatrix} \begin{pmatrix}a_5 \\ a_4 \\ a_3 \\ a_2 \\ a_1 \\ a_0 \end{pmatrix}. \end{align}
In [17]:
qi = 0;
qf = 1;
dqi = 0.0;
dqf = 0;
ddqi = 0;
ddqf = 0;
t = 50;
ts = [0:0.01:t];
We form the matrix $$ \boldsymbol{\mathrm A} = \begin{pmatrix} 0 & 0 & 0 & 0 & 0 & 1 \\ t^5 & t^4 & t^3 & t^2 & t & 1 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 5t^4 & 4t^3 & 3t^2 & 2t & 1 & 0 \\ 0 & 0 & 0 & 2 & 0 & 0 \\ 20t^3 & 12t^2 & 6t & 2 & 0 & 0 \end{pmatrix} $$
In [18]:
A = [0, 0, 0, 0, 0, 1;
t^5, t^4, t^3, t^2, t, 1;
0, 0, 0, 0, 1, 0;
5*t^4, 4*t^3, 3*t^2, 2*t, 1, 0;
0, 0, 0, 2, 0, 0;
20*t^3, 12*t^2, 6*t, 2, 0, 0]
and the the vector $$\boldsymbol{\mathrm{b}} = \begin{pmatrix} q_i \\ q_f \\ \dot q_i \\ \dot q_f \\ \ddot q_i \\ \ddot q_f \end{pmatrix}.$$
In [19]:
b = [qi, qf, dqi, dqf, ddqi, ddqf]'
We then solve for the polynomial coeffiecient vector $$\boldsymbol{\mathrm{p}} = \begin{pmatrix} a_5 \\ a_4 \\ a_3 \\ a_2 \\ a_1 \\ a_0 \end{pmatrix}.$$
In [20]:
p = A \ b
In [21]:
plot(ts, polyval(p, ts), 'linewidth',2);
title ("Quintic Trajectory");
xlabel ("Time");
ylabel ("position");
In [22]:
plot(ts, polyval([5 * p(1), 4 * p(2), 3 * p(3), 2 * p(4), p(5)], ts),
'linewidth', 2, 'color', [0, 0.5, 0])
title ("Quintic Trajectory");
xlabel ("Time");
ylabel ("velocity");
In [23]:
plot(ts, polyval([20 * p(1), 12 * p(2), 6 * p(3), 2 * p(4)], ts),
'linewidth',2, 'color', [1,0,0])
title ("Quintic Trajectory");
xlabel ("Time");
ylabel ("Acceleration");
We can also create a function that returns the trajectory
In [24]:
function [ts, qs, dqs, ddqs] = quintic_trajectory(qi, qf, dqi, dqf, ddqi, ddqf, t)
b = [qi, qf, dqi, dqf, ddqi, ddqf]';
A = [0, 0, 0, 0, 0, 1;
t^5, t^4, t^3, t^2, t, 1;
0, 0, 0, 0, 1, 0;
5*t^4, 4*t^3, 3*t^2, 2*t, 1, 0;
0, 0, 0, 2, 0, 0;
20*t^3, 12*t^2, 6*t, 2, 0, 0];
p = A \ b;
ts = [0:0.01:t];
qs = polyval(p, ts);
dqs = polyval([5 * p(1), 4 * p(2), 3 * p(3), 2 * p(4), p(5)], ts);
ddqs = polyval([20 * p(1), 12 * p(2), 6 * p(3), 2 * p(4)], ts);
end
In [25]:
[ts, qs, dqs, ddqs] = quintic_trajectory(0, 1, 0.5, 0, 0, 0, 10);
plot(ts,qs, 'linewidth', 2, ts,dqs, 'linewidth', 2, ts, ddqs, 'linewidth', 2)
This type of trajectory is appropriate when a constant velocity is desired along a portion of the path.
The LSPB trajectory is such that the velocity is initially “ramped up” to its desired value and then “ramped down” when it approaches the goal position, resulting in a trapezoidal velocity profile.
To achieve this we specify the desired trajectory in three parts:
We are given the intitial position $q(0) = q_i$, the final position $q(t_f) = q_f$ and the duration of the motion.
The trajectory has to satisfy some constraints to ensure the transition from $q_i$ to $q_f$ in a time $t_f$. The velocity at the end of the parabolic segment must be equal to the (constant) velocity of the linear segment, that is, $$\begin{align} \ddot q_c t_c &= \frac{q_m - q_c}{t_m - t_c} \\ q_m &= \frac{q_f + q_i}{2} \\ t_m &= t_f / 2 . \end{align} $$
The joint position $q_c$ after the ramp-up with constant acceleration is found $$ q_c = q_i + \frac{1}{2} \ddot q_c t_c^2. $$
The blend time $t_c$ can then be found by solving $$\ddot q_c t_c^2 - \ddot q_c t_c t_f + q_f - q_i = 0$$ for $t_c$. This gives $$t_c = \frac{t_f}{2} - \frac{1}{2}\sqrt{\frac{t_f^2 \ddot q_c - 4(q_f - q_i)}{\ddot q_c}}.$$
Acceleration is then subject to the constraint $$|\ddot q_c| \geq \frac{4|q_f - q_i|}{t_f^2}$$
The polynomials for the whole trajectory can then be found: $$q(t) = \begin{cases} q_i + \frac{1}{2}\ddot q_c t^2 & 0 \leq t \leq t_c \\ q_i + \ddot q_c t_c (t - t_c / 2) & t_c \leq t \leq t_f - t_c \\ q_f - \frac{1}{2}\ddot q_c(t_f - t)^2 & t_f - t_c \leq t \leq t_f \\ \end{cases} $$
In [26]:
qi = 0
qf = pi
tf = 1
ddq_abs = 6 * pi
Check the acceleration constraint $$|\ddot q_c| \geq \frac{4|q_f - q_i|}{t_f^2}.$$
In [27]:
ddq_min = 4 * abs(qf - qi) / tf^2
In [28]:
ddq_abs > ddq_min
Compute the blend time using $$t_c = \frac{t_f}{2} - \frac{1}{2}\sqrt{\frac{t_f^2 \ddot q_c - 4(q_f - q_i)}{\ddot q_c}}.$$
In [29]:
tc = tf/2 - 0.5 * sqrt((tf^2*ddq_abs - 4*(qf-qi)) / ddq_abs)
Check the maximum blend time
In [30]:
tc_max = tf/2 - 0.5 * sqrt((tf^2*ddq_min - 4*(qf-qi)) / ddq_min)
As seen the maximum blend time is half the duration of the motion
First segment (ramp-up):
In [31]:
ts_ramp = [0:0.01:tc];
qs_rampup = qi + 0.5 * ddq_abs * ts_ramp.^2;
dqs_rampup = ddq_abs * ts_ramp;
ddqs_rampup = repmat(ddq_abs,1,size(qs_rampup)(2));
Middle segment (constant velocity):
In [32]:
ts_mid = [tc:0.01:tf-tc];
qs_mid = qi + ddq_abs * tc * (ts_mid - 0.5 * tc);
len = size(qs_mid)(2);
dqs_mid = repmat(ddq_abs*tc,1,len);
ddqs_mid = repmat(0,1,len);
Final segment (ramp down):
In [33]:
ts_rampdown = [tf-tc:0.01:tf];
qs_rampdown = qf - 0.5 * ddq_abs * (tf - ts_rampdown).^2;
dqs_rampdown = ddq_abs * tc - ddq_abs * ts_ramp;
ddqs_rampdown = repmat(-ddq_abs,1,size(qs_rampup)(2));
Assemble the whole trajectory
In [34]:
ts = [ts_ramp, ts_mid, ts_rampdown];
qs = [qs_rampup, qs_mid, qs_rampdown];
dqs = [dqs_rampup, dqs_mid, dqs_rampdown];
ddqs = [ddqs_rampup, ddqs_mid, ddqs_rampdown];
plot(ts, qs, 'linewidth', 2,
ts, dqs, 'linewidth', 2,
ts, ddqs, 'linewidth', 2);
In [35]:
function [ts, qs, dqs, ddqs] = lspb_trajectory(qi, qf, tf, acc_scale)
ddq_min = 4 * abs(qf - qi) / tf^2;
ddq_abs = ddq_min * acc_scale;
tc = tf/2 - 0.5 * sqrt((tf^2*ddq_abs - 4*(qf-qi)) / ddq_abs);
ts_ramp = [0:0.01:tc];
qs_rampup = qi + 0.5 * ddq_abs * ts_ramp.^2;
dqs_rampup = ddq_abs * ts_ramp;
ddqs_rampup = repmat(ddq_abs,1,size(qs_rampup)(2));
ts_mid = [tc:0.01:tf-tc];
qs_mid = qi + ddq_abs * tc * (ts_mid - 0.5 * tc);
len = size(qs_mid)(2);
dqs_mid = repmat(ddq_abs*tc,1,len);
ddqs_mid = repmat(0,1,len);
ts_rampdown = [tf-tc:0.01:tf];
qs_rampdown = qf - 0.5 * ddq_abs * (tf - ts_rampdown).^2;
dqs_rampdown = ddq_abs * tc - ddq_abs * ts_ramp;
ddqs_rampdown = repmat(-ddq_abs,1,size(qs_rampup)(2));
ts = [ts_ramp, ts_mid, ts_rampdown];
qs = [qs_rampup, qs_mid, qs_rampdown];
dqs = [dqs_rampup, dqs_mid, dqs_rampdown];
ddqs = [ddqs_rampup, ddqs_mid, ddqs_rampdown];
end
In [36]:
[ts, qs, dqs, ddqs] = lspb_trajectory(0,1,1,1.2);
plot(ts, qs, 'linewidth', 2,
ts, dqs, 'linewidth', 2,
ts, ddqs, 'linewidth', 2);
We choose the blend time $t_b$ so that the position curve is symmetric. Suppose that $t_0 = 0$ and $\dot q(t_f) = 0 = \dot{q}(0)$.
Then between times $0$ and $t_b$ we have $$ q(t) = a_0 + a_1 t + a_2 t^2 $$ with velocity $$\dot q(t) = a_1 + 2 a_2 t.$$
Since $q_0 = 0$ and $\dot q(0)$: \begin{align} a_0 = q_0 a_1 = 0 \end{align}
At time $t_b$ the velocity should be equal to a given constant $V$, thus, $$a_2 = \frac{V}{2t_b}.$$
From symmetry: $$q\left( \frac{t_f}{2} \right) = \frac{q_0 + q_f}{2}$$ Thus, $$\frac{q_0 + q_f}{2} = a_0 + V\frac{t_f}{2}$$
The two segments blend at time $t_b$: $$q_0 + \frac{V}{2} t_b = \frac{q_0 + q_f - V t_f}{2} + V t_b$$
The blend time is then:
The velocity must satisfy $$\frac{q_f - q_i}{t_f} < V \leq \frac{2(q_f - q_i)}{t_f} $$
The whole trajectory is then
Here, $\dot q = V$ and $\ddot q = \alpha$.
In [102]:
function [q, dq, ddq] = lspb_trajectory(q0, q1, t)
t0 = t;
t = (0:t-1)';
tf = max(t(:));
V = (q1 - q0)/tf * 1.5;
tb = (q0 - q1 + V*tf)/V;
a = V/tb;
q = zeros(length(t),1);
dq = q;
ddq = q;
for i = 1:length(t)
tt = t(i);
if tt <= tb
q(i) = q0 + a/2*tt^2;
dq(i) = a*tt;
ddq(i) = a;
elseif tt <= (tf-tb)
q(i) = (q1+q0-V*tf)/2 + V*tt;
dq(i) = V;
ddq(i) = 0;
else
q(i) = q1 -a/2*tf^2 + a*tf*tt - a/2*tt^2;
dq(i) = a*tf - a*tt;
ddq(i) = -a;
end
end
end
In [38]:
[q, dq, ddq] = lspb_trajectory(0,100,100);
plot(q, 'linewidth', 2)
title ("LSPB Trajectory");
xlabel ("Time");
ylabel ("Position");
In [39]:
plot(dq, 'linewidth', 2, 'color', [0,0.5,0])
title ("LSPB Trajectory");
xlabel ("Time");
ylabel ("Velocity");
In [40]:
plot(ddq, 'linewidth', 2, 'color', 'r')
title ("LSPB Trajectory");
xlabel ("Time");
ylabel ("Acceleration");
In several applications, the path is described in terms of a number of points greater than two.
For instance, even for the simple point-to-point motion of a pick-and-place task, it may be worth assigning two intermediate points between the initial point and the final point; suitable positions can be set for lifting off and setting down the object, so that reduced velocities are obtained with respect to direct transfer of the object.
In [41]:
t = [0;2;4];
In [46]:
q_v = [0 2 3];
In [48]:
scatter(t, q_v, 'red', 'filled');
In [49]:
D_t = 0.3*ones(size(t));
In [50]:
dq_i = 0;
dq_f = 0;
In [51]:
Ts = 0.01;
In [52]:
[time,q,dq,ddq] = lin_par(t,q_v,D_t,dq_i,dq_f,Ts);
In [53]:
scatter(t, q_v, 'red', 'filled');
hold on;
plot(time, q, 'linewidth', 2)
title ("Motion Through a Sequence of Points");
xlabel ("Time");
ylabel ("Position");
In [54]:
plot(time, dq, 'linewidth', 2, 'color', [0,0.5,0])
title ("Motion Through a Sequence of Points");
xlabel ("Time");
ylabel ("Velocity");
In [55]:
plot(time, ddq, 'linewidth', 2, 'color', 'red')
title ("Motion Through a Sequence of Points");
xlabel ("Time");
ylabel ("Acceleration");
A joint space trajectory planning algorithm generates a time sequence of values for the joint variables $q(t)$ so that the manipulator is taken from the initial to the final configuration.
The resulting end-effector motion is not easily predictable.
Whenever it is desired that the end-effector motion follows a geometrically specified path in the operational space, it is necessary to plan trajectory execution directly in the same space.
Let $\boldsymbol{\mathrm p}_i \in \mathbb R^3$ be the start position and let $\boldsymbol{\mathrm p}_f \in \mathbb R^3$ be the final position. A position vector $\boldsymbol{\mathrm p}$ can then be written $$\boldsymbol{\mathrm p} = \begin{pmatrix} x \\ y \\ z \end{pmatrix}, $$ where $x,y,z \in \mathbb R$.
Consider the vector function $$ \boldsymbol{\mathrm p} = \boldsymbol{\mathrm f}(\sigma), $$ with $\sigma \in [\sigma_i, \sigma_f]$. The sequence of values of $\boldsymbol{\mathrm p}$ is called a path in space.
We employ the arc length $s$ as the parameter. The start point is then $$\boldsymbol{\mathrm p}_i = \boldsymbol{\mathrm f}(0)$$ and the end point is $$\boldsymbol{\mathrm p}_f = \boldsymbol{\mathrm f}(s).$$
Again, Let $\boldsymbol{\mathrm p}_i \in \mathbb R^3$ be the start position and let $\boldsymbol{\mathrm p}_f \in \mathbb R^3$ be the final position. Consider the linear segment connecting $\boldsymbol{\mathrm p}_i$ and $\boldsymbol{\mathrm p}_f$.
The parametric representation of this path is $$ \boldsymbol{\mathrm p}(s) = \boldsymbol{\mathrm p}_i + \frac{\boldsymbol{\mathrm p}_f - \boldsymbol{\mathrm p}_i}{\|\boldsymbol{\mathrm p}_f - \boldsymbol{\mathrm p}_i\|} s.$$
Here $$\boldsymbol{\mathrm p}(0) = \boldsymbol{\mathrm p}_i$$ and $$\boldsymbol{\mathrm p}(\|\boldsymbol{\mathrm p}_f - \boldsymbol{\mathrm p}_i\|) = \boldsymbol{\mathrm p}_f$$
Differentiating $\boldsymbol{\mathrm p}(s)$ with respect to $s$ gives: \begin{align} \frac{d \boldsymbol{\mathrm p}}{ds} &= \frac{\boldsymbol{\mathrm p}_f - \boldsymbol{\mathrm p}_i}{\|\boldsymbol{\mathrm p}_f - \boldsymbol{\mathrm p}_i\|} \\ \frac{d^2 \boldsymbol{\mathrm p}}{ds^2} &= 0 \end{align}
In [59]:
pi = [0,0,0]';
pf = [1,2,3]';
In [60]:
length = norm(pf-pi)
In [63]:
direction = (pf - pi) / length
norm(direction)
In [68]:
[xs, dxs, ddxs] = lspb_trajectory(pi(1), pf(1), 50);
[ys, dys, ddys] = lspb_trajectory(pi(2), pf(2), 50);
[zs, dzs, ddzs] = lspb_trajectory(pi(3), pf(3), 50);
In [72]:
plot(xs, 'linewidth',2); hold on;
plot(ys, 'linewidth',2, 'color', [0, 0.5, 0])
plot(zs, 'linewidth',2, 'color', 'red')
Consider the end-effector orientation. The orientation can be represented by a rotation matrix $\boldsymbol{\mathrm R}$ of the end-effector frame with respect to the base-frame.
We first consider orientation represented by Euler angles, e.g., XYZ Euler angles.
The rotation matrix around the z-axis is: $$\boldsymbol{\mathrm R}_z = \begin{pmatrix} \cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{pmatrix} $$
In [107]:
function R = rotz(theta)
ct = cos(theta);
st = sin(theta);
R = [ct, -st, 0;
st, ct, 0;
0, 0, 1];
end
In [109]:
R = rotz(pi/6)
The rotation matrix around the y-axis is: $$\boldsymbol{\mathrm R}_z = \begin{pmatrix} \cos \theta & 0 & \sin \theta \\ 0 & 1 & 0 \\ -\sin \theta & 0 & \cos \theta \end{pmatrix} $$
In [111]:
function R = roty(th)
ct = cos(th);
st = sin(th);
R = [ct, 0, st;
0, 1, 0;
-st, 0, ct];
end
In [112]:
Ry = roty(pi/6)
The rotation matrix around the y-axis is: $$\boldsymbol{\mathrm R}_z = \begin{pmatrix} 1 & 0 & 0 \\ 0 & \cos \theta & \sin \theta \\ 0 & -\sin \theta & \cos \theta \end{pmatrix} $$
In [119]:
function R = rotx(th)
ct = cos(th);
st = sin(th);
R = [1, 0, 0;
0, ct, -st;
0, st, ct];
end
In [123]:
rpy = [1,2,3];
In [125]:
R = rotx(rpy(1)) * roty(rpy(2)) * rotz(rpy(3))
In [118]:
rotx(1)
In [ ]: