Task Trajectory Interpolation

Interpolation with Cubic Spline

IndySDK allows users to modify trajectory generation methods as well as low-level controllers. This example describe how to develop joint trajectory interpolators with cubic splines (For more details on a cubic spline, refer to Joint Trajectory Interpolation Example). Note that the basic contents of this example are similar to those of joint trajectory interpolation example, but it may be unfamiliar to implement the trajectory interpolation for task-space rotations.

Therefore, this example focuses on the following item.

• How to implement the task-space interpolation using exponential mapping.

Example Code

Generate the task trajectory interpolation component and define several functions and variables in the header as follows:

//interpolate paths
void Interpolate(Eigen::MatrixXd *a);

// coefficients: q(s) = a0*s^3 + a1*s^2 + a2*s + a3
Eigen::MatrixXd *disp_a;
Eigen::MatrixXd *rot_a;

LieGroup::Rotation  _R0;
double  _t0;
double  _s;
double  _nSegment;
double  _segTime;
PosType _readyPos;

bool    _isTargetReached = false;
bool    _isTrajSet = false;

std::vector<double> _distDisp;
std::vector<double> _distRot;

std::vector<LieGroup::Vector3D> _Waypoint;
int nPts;
int nSplines;
int nCoeffs;
int dim;

Next, initialize pointer variables by allocating memories as follows:

TaskTrajectoryInterpolationComponent::TaskTrajectoryInterpolationComponent()
: AbstractFPathInterpolator<3>(USERNAME, EMAIL, SERIAL)
, nCoeffs(4)
{
    disp_a = new Eigen::MatrixXd[nCoeffs];
    rot_a = new Eigen::MatrixXd[nCoeffs];
}

When closing the program, releasing memories is implemented with the following code:

TaskTrajectoryInterpolationComponent::~TaskTrajectoryInterpolationComponent()
{
    if (disp_a != NULL)
        delete disp_a;

    if (rot_a != NULL)
        delete rot_a;
}

Task Trajectory Interpolation Component consists of setPath and traj functions. First, setPath is used to set the path with waypoints defined by the user interface (e.g., Conty). traj generates the trajectory to be used for the controller at every sampling time.

Fill in the setPath function based on the waypoints given in the form of vector-type pointer:

void TaskTrajectoryInterpolationComponent::setPath(const PosType *path,
const int len, double * maxDeviation, const int *dirAngle,
bool simulCheck)
{
    std::vector<LieGroup::Vector3D> wp_disp;
    std::vector<LieGroup::Vector3D> wp_rot;

    LieGroup::Rotation R;
    dim = 3;

    _readyPos = path[0];
    printf("pos: %f||%f||%f\n", _readyPos.r()[0], _readyPos.r()[1], _readyPos.r()[2]);
    wp_disp.push_back(path[0].r());
    _R0 = path[0].R();
    wp_rot.push_back(LieGroup::Vector3D::Zero());

    for (int i = 1; i < len; i++)
    {
        printf("pos: %f||%f||%f\n", _readyPos.r()[0], _readyPos.r()[1], _readyPos.r()[2]);

        wp_disp.push_back(path[i].r());
        R = _R0.icascade(path[i].R());
        wp_rot.push_back(R.expCoord());
        _distDisp.push_back((wp_disp[i] - wp_disp[i-1]).norm());
        _distRot.push_back((wp_rot[i] - wp_rot[i-1]).norm());
    }

    _Waypoint = wp_disp;
    nPts = wp_disp.size();
    nSplines = wp_disp.size()-1;

    for (int i=0; i < nCoeffs; i++)
    {
        disp_a[i].resize(dim, nSplines);
        rot_a[i].resize(dim, nSplines);
    }

    Interpolate(disp_a);

    _nSegment = len-1;
}

This example uses a cubic spline to set the path between waypoints: The coefficients that satisfy specific conditions can be calculated by constructing a specific matrix given as follow (Each bundle of code indicated by comments represents which part describes which condition).

void TaskTrajectoryInterpolationComponent::Interpolate(Eigen::MatrixXd *a)
{
    Eigen::MatrixXd coeff_A;
    Eigen::MatrixXd coeff_b;
    Eigen::MatrixXd coeff;
    for (int j=0; j<dim; j++){
        coeff_A = Eigen::MatrixXd::Zero(4*nSplines,4*nSplines);
        coeff_b = Eigen::MatrixXd::Zero(4*nSplines,1);
        coeff = Eigen::MatrixXd::Zero(4*nSplines,1);
        for (int i=0; i<nSplines; i++){
            // N  = nSplines
            // N conditions: x = f_i(s_i)
            coeff_A(i,i*4+0) = pow(i,3);
            coeff_A(i,i*4+1) = pow(i,2);
            coeff_A(i,i*4+2) = i;
            coeff_A(i,i*4+3) = 1.0;
            coeff_b(i,0) = _Waypoint[i][j];

            // N conditions: x = f_i(s_(i+1))
            coeff_A(nSplines+i,i*4+0) = pow(i+1,3);
            coeff_A(nSplines+i,i*4+1) = pow(i+1,2);
            coeff_A(nSplines+i,i*4+2) = i+1;
            coeff_A(nSplines+i,i*4+3) = 1.0;
            coeff_b(nSplines+i,0) = _Waypoint[i+1][j];

            // 2*(N-1) conditions: f_i'(s_(i+1))
            //= f_(i+1)'(s_(i+1)), f_i''(s_(i+1)) = f_(i+1)''(s_(i+1))
            if(i<nSplines-1){
                coeff_A(2*nSplines+i, i*4+0) = 3.0*pow(i+1,2);
                coeff_A(2*nSplines+i, i*4+1) = 2.0*(i+1);
                coeff_A(2*nSplines+i, i*4+2) = 1.0;
                coeff_A(2*nSplines+i,(i+1)*4+0) = -3.0*pow(i+1,2);
                coeff_A(2*nSplines+i,(i+1)*4+1) = -2.0*(i+1);
                coeff_A(2*nSplines+i,(i+1)*4+2) = -1.0;
                coeff_b(2*nSplines+i,0) = 0.0;

                coeff_A(3*nSplines-1+i, i*4+0) = 6.0*(i+1);
                coeff_A(3*nSplines-1+i, i*4+1) = 2.0;
                coeff_A(3*nSplines-1+i,(i+1)*4+0) = -6.0*(i+1);
                coeff_A(3*nSplines-1+i,(i+1)*4+1) = -2.0;
                coeff_b(3*nSplines-1+i,0) = 0.0;
            }

            // 2 conditions: f_0''(s_0) = 0, f_n''(s_(n+1)) = 0;
            coeff_A(4*nSplines-2, 0*4+0) = 6.0*0;
            coeff_A(4*nSplines-2, 0*4+1) = 2.0;
            coeff_b(4*nSplines-2, 0) = 0.0;

            coeff_A(4*nSplines-1, (nSplines-1)*4+0) = 6.0*nSplines;
            coeff_A(4*nSplines-1, (nSplines-1)*4+1) = 2.0;
            coeff_b(4*nSplines-1, 0) = 0.0;
        }

        coeff = coeff_A.inverse()*coeff_b;
        for (int i = 0; i<nSplines; i++){
            a[0](j,i) = coeff(4*i);
            a[1](j,i) = coeff(4*i+1);
            a[2](j,i) = coeff(4*i+2);
            a[3](j,i) = coeff(4*i+3);
        }
    }
}

Next, define parameters to generate the trajectory in setTraj function. In this example, the time required to move between the waypoints is set to 10 seconds.

void TaskTrajectoryInterpolationComponent::setTraj(double t0)
{
    _t0 = t0;
    _segTime = 10.0;
    _isTargetReached = false;
    _isTrajSet = true;
}

Now, fill in the traj function to generate real-time trajectory. Based on the coefficients calculated from Interpolate() function, desired position, desired velocity, and desired acceleration are calculated. Note that this example uses a linear function with a slope of 1/10 because arbitrary integers (i.e., s=0, 1, 2, ...) were used at waypoints when generating the path (note that the objective is to make the robot reach the target point after a certain period of time).

void TaskTrajectoryInterpolationComponent::traj(double time, PosType & pos,
VelType & vel, AccType & acc)
{
  if (time<=_t0)
  {
      pos = _readyPos;
      vel = VelType::Zero();
      acc = AccType::Zero();
      return;
  }

  _s = (time-_t0)/_segTime;
  double sdot = 1.0/_segTime;
  double sddot = 0.0;

  if (_s > nSplines) _s = nSplines;

  int idx;
  if (_s < nSplines)
  {
        idx = (int)floor(_s);
  }else
  {
        idx = nSplines-1;
  }

  LieGroup::Vector3D xi_d, xidot_d, xiddot_d;
  for (int i=0; i< dim; i++)
  {
        pos.r()[i] = disp_a[0](i, idx)*pow(_s, 3)
                     + disp_a[1](i, idx)*pow(_s, 2)
                     + disp_a[2](i, idx)*_s + disp_a[3](i, idx);
        vel.v()[i] = (3.0*disp_a[0](i, idx)*pow(_s, 2)
                     + 2.0*disp_a[1](i, idx)*_s + disp_a[2](i, idx))*sdot;
        acc.v()[i] = (3.0*disp_a[0](i, idx)*pow(_s, 2)
                     + 2.0*disp_a[1](i, idx)*_s + disp_a[2](i, idx))*sddot
                     + (6.0*disp_a[0](i, idx)*_s
                     + 2.0*disp_a[1](i, idx))*sdot*sdot;

        xi_d[i] = rot_a[0](i, idx)*pow(_s, 3) + rot_a[1](i, idx)*pow(_s, 2)
                  + rot_a[2](i, idx)*_s + rot_a[3](i, idx);
        xidot_d[i] = (3.0*rot_a[0](i, idx)*pow(_s, 2)
                      + 2.0*rot_a[1](i, idx)*_s + rot_a[2](i, idx))*sdot;
        xiddot_d[i] = (3.0*rot_a[0](i, idx)*pow(_s, 2)
                       + 2.0*rot_a[1](i, idx)*_s + rot_a[2](i, idx))*sddot
                       + (6.0*rot_a[0](i, idx)*_s
                       + 2.0*rot_a[1](i, idx))*sdot*sdot;
  }

  LieGroup::Rotation exp = xi_d.exp();
  Eigen::Matrix3d dexp = xi_d.dexp();
  Eigen::Matrix3d ddt_dexp = xi_d.ddt_dexp(xidot_d);

  pos.R() = _R0*exp;
  vel.w() = dexp.transpose()*xidot_d;
  acc.w() = dexp.transpose()*xiddot_d + ddt_dexp.transpose()*xidot_d;

  if ( _t0 +  _nSegment*_segTime <= time)
  {
        _readyPos = pos;
        _isTrajSet = false;
        _isTargetReached = true;
        _s = 0.0;
  }
}

Finally, the following functions are used so that the controller can bring the parameters from the interpolator.

• setInitialTime brings the control time to define the starting time of the trajectory.

void TaskTrajectoryInterpolationComponent::setInitialTime(double t0)
{
    _t0 = t0;
}

• getDuration allows the controller to get the time required to finish the generated trajectory.

double TaskTrajectoryInterpolationComponent::getDuration()
{
    return _nSegment*_segTime;
}

• getNTotalSegment allows the controller to call the number of waypoints.

int TaskTrajectoryInterpolationComponent::getNTotalSegment()
{
    return _nSegment;
}

• getCurrentWaypoint allows the controller to call the waypoint the robot is passing through.

int TaskTrajectoryInterpolationComponent::getCurrentWaypoint()
{
    return floor(_s);
}

• isTargetReached allows the controller to check whether the robot arrives at the target point.

bool TaskTrajectoryInterpolationComponent::isTargetReached()
{
    return _isTargetReached;
}