import jax.numpy as jnp
import abc
[docs]
class Trajectory(abc.ABC):
"""
Base class for trajectory motion generation.
"""
def __init__(self, n: int, sampling: int, ti: float, tf: float):
self.n = n
self.sampling = sampling
self.ti = ti
self.tf = tf
self.time = jnp.linspace(ti, tf, sampling)
[docs]
@abc.abstractmethod
def get_value(self, t: float):
pass
[docs]
@abc.abstractmethod
def compute_with_constraints(self, qmin, qmax, qpmin, qpmax, qppmin, qppmax):
pass
[docs]
@abc.abstractmethod
def compute_full_trajectory(self):
pass
[docs]
class SplineTrajectory(Trajectory):
"""
Spline-based trajectory.
.. todo::
Implement the compute_with_constraints function
"""
def __init__(self, ndof, sampling, ti, tf, control_points):
super().__init__(ndof, sampling, ti, tf)
self.control_points = jnp.array(control_points)
[docs]
def get_value(self, t: float):
return jnp.interp(t, self.time, self.control_points)
[docs]
def compute_full_trajectory(self):
traj_per_dof = jnp.interp(self.time, self.time, self.control_points)
traj = jnp.tile(traj_per_dof, (self.n, 1)).T
return traj
[docs]
def compute_with_constraints(self, qmin, qmax, qpmin, qpmax, qppmin, qppmax):
return super().compute_with_constraints(qmin, qmax, qpmin, qpmax, qppmin, qppmax)
[docs]
class TrapezoidalTrajectory(Trajectory):
"""
Trapezoidal velocity profile trajectory.
.. todo::
Implment the compute_with_constraints function
"""
def __init__(self, n, sampling, ti, tf, q0, qf, acc, vel):
super().__init__(n, sampling, ti, tf)
self.q0 = jnp.array(q0)
self.qf = jnp.array(qf)
self.acc = jnp.array(acc)
self.vel = jnp.array(vel)
[docs]
def get_value(self, t: float):
return self.q0 + (self.qf - self.q0) * (t - self.ti) / (self.tf - self.ti)
[docs]
def compute_full_trajectory(self):
return self.get_value(self.time)
[docs]
def compute_with_constraints(self, qmin, qmax, qpmin, qpmax, qppmin, qppmax):
return super().compute_with_constraints(qmin, qmax, qpmin, qpmax, qppmin, qppmax)
[docs]
class PeriodicTrajectory(Trajectory):
"""
Periodic trajectory based on Fourier series.[1]
.. todo::
Implment the compute_with_constraints function
Ref:
- [1] Fourier-based optimal excitation trajectories for the dynamic
identification of robots, Kyung.Jo Park - Robotica - 2006.
"""
def __init__(self, n, sampling, ti, tf, frequency, Aij, Bij, nb_terms):
super().__init__(n, sampling, ti, tf)
self.frequency = frequency
self.Aij = jnp.array(Aij)
self.Bij = jnp.array(Bij)
self.nb_terms = nb_terms
[docs]
def get_value(self, t: float):
omega = 2 * jnp.pi * self.frequency
q = jnp.zeros(self.ndof)
for i in range(self.ndof):
for j in range(1, self.nb_terms + 1):
q = q + (self.Aij[i, j-1] * jnp.cos(omega * j * t) +
self.Bij[i, j-1] * jnp.sin(omega * j * t))
return q
[docs]
def compute_full_trajectory(self):
return jnp.array([self.get_value(t) for t in self.time])
[docs]
def compute_with_constraints(self, qmin, qmax, qpmin, qpmax, qppmin, qppmax):
return super().compute_with_constraints(qmin, qmax, qpmin, qpmax, qppmin, qppmax)
[docs]
class StepTrajectory(Trajectory):
"""
Step trajectory with fixed small duration epsilon and given amplitude.
.. todo::
Implment the compute_with_constraints function
"""
def __init__(self, ndof, sampling, ti, tf, epsilon, amplitude):
super().__init__(ndof, sampling, ti, tf)
self.epsilon = epsilon
self.amplitude = amplitude
[docs]
def get_value(self, t: float):
return jnp.where(t % (2 * self.epsilon) < self.epsilon, self.amplitude, 0)
[docs]
def compute_full_trajectory(self)->jnp.array:
return jnp.array([self.get_value(t) for t in self.time])
[docs]
def compute_with_constraints(self, qmin, qmax, qpmin, qpmax, qppmin, qppmax):
return super().compute_with_constraints(qmin, qmax, qpmin, qpmax, qppmin, qppmax)