Program Listing for File trajectory_spline_sampler.h
↰ Return to documentation for file (/tmp/ws/src/robot_controllers/robot_controllers/include/robot_controllers/trajectory_spline_sampler.h)
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2014, Fetch Robotics Inc.
* Copyright (c) 2013, Unbounded Robotics Inc.
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Unbounded Robotics, Willow Garage nor the names
* of their contributors may be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
// Portions are based on splines.h of arm_navigation
// Author: Michael Ferguson, Mrinal Kalakrishnan (splines.h)
#ifndef ROBOT_CONTROLLERS__TRAJECTORY_SPLINE_SAMPLER_H_
#define ROBOT_CONTROLLERS__TRAJECTORY_SPLINE_SAMPLER_H_
#include <vector>
#include "robot_controllers/trajectory.h"
namespace robot_controllers
{
struct Spline
{
double coef[6];
};
enum SplineType
{
QUINTIC,
CUBIC,
LINEAR
};
static inline void generatePowers(int n, double x, double* powers)
{
powers[0] = 1.0;
for (int i = 1; i <= n; ++i)
{
powers[i] = powers[i-1]*x;
}
}
static void QuinticSpline(double p0, double v0, double a0,
double p1, double v1, double a1,
double t, Spline& s)
{
if (t == 0.0)
{
s.coef[0] = p1;
s.coef[1] = v1;
s.coef[2] = 0.5*a1;
s.coef[3] = 0.0;
s.coef[4] = 0.0;
s.coef[5] = 0.0;
}
else
{
double T[6];
generatePowers(5, t, T);
s.coef[0] = p0;
s.coef[1] = v0;
s.coef[2] = 0.5*a0;
s.coef[3] = (-20.0*p0 + 20.0*p1 - 3.0*a0*T[2] + a1*T[2] -
12.0*v0*T[1] - 8.0*v1*T[1]) / (2.0*T[3]);
s.coef[4] = (30.0*p0 - 30.0*p1 + 3.0*a0*T[2] - 2.0*a1*T[2] +
16.0*v0*T[1] + 14.0*v1*T[1]) / (2.0*T[4]);
s.coef[5] = (-12.0*p0 + 12.0*p1 - a0*T[2] + a1*T[2] -
6.0*v0*T[1] - 6.0*v1*T[1]) / (2.0*T[5]);
}
}
static void sampleQuinticSpline(Spline& s, double t,
double& position, double& velocity, double& acceleration)
{
double T[6];
generatePowers(5, t, T);
position = T[0]*s.coef[0] +
T[1]*s.coef[1] +
T[2]*s.coef[2] +
T[3]*s.coef[3] +
T[4]*s.coef[4] +
T[5]*s.coef[5];
velocity = T[0]*s.coef[1] +
2.0*T[1]*s.coef[2] +
3.0*T[2]*s.coef[3] +
4.0*T[3]*s.coef[4] +
5.0*T[4]*s.coef[5];
acceleration = 2.0*T[0]*s.coef[2] +
6.0*T[1]*s.coef[3] +
12.0*T[2]*s.coef[4] +
20.0*T[3]*s.coef[5];
}
static void CubicSpline(double p0, double v0, double p1, double v1, double t, Spline& s)
{
if (t == 0.0)
{
s.coef[0] = p1;
s.coef[1] = v1;
s.coef[2] = 0.0;
s.coef[3] = 0.0;
}
else
{
double T[4];
generatePowers(3, t, T);
s.coef[0] = p0;
s.coef[1] = v0;
s.coef[2] = (3.0 * p1 - 3.0 * p0 - 2.0 *v0*T[1] - v1*T[1]) / T[2];
s.coef[3] = (-2.0 * p1 + 2.0 * p0 + v0*T[1] + v1*T[1]) / T[3];
}
}
static void sampleCubicSpline(Spline s, double t, double& position, double& velocity)
{
double T[4];
generatePowers(3, t, T);
position = T[0]*s.coef[0] +
T[1]*s.coef[1] +
T[2]*s.coef[2] +
T[3]*s.coef[3];
velocity = T[0]*s.coef[1] +
2.0*T[1]*s.coef[2] +
3.0*T[2]*s.coef[3];
}
static void LinearSpline(double p0, double p1, double t, Spline& s)
{
s.coef[0] = p0;
s.coef[1] = p1 - p0;
s.coef[2] = t;
}
static void sampleLinearSpline(Spline& s, double t, double& position)
{
position = s.coef[0] + (s.coef[1] * t/s.coef[2]);
}
class SplineTrajectorySampler : public TrajectorySampler
{
struct Segment
{
double start_time;
double end_time;
int type;
std::vector<Spline> splines;
};
public:
explicit SplineTrajectorySampler(const Trajectory& trajectory) :
trajectory_(trajectory)
{
// Check for invalid trajectory size
if (trajectory.size() == 0)
{
throw 0;
}
// Check for number of joints
size_t num_joints = trajectory.points[0].q.size();
// Trajectory length one is special
if (trajectory.size() == 1)
{
segments_.resize(1);
segments_[0].start_time = segments_[0].end_time = trajectory.points[0].time;
// Set up spline
segments_[0].splines.resize(num_joints);
for (size_t j = 0; j < num_joints; ++j)
{
LinearSpline(trajectory.points[0].q[j],
trajectory.points[0].q[j],
0.0,
segments_[0].splines[j]);
}
segments_[0].type = LINEAR;
result.q.resize(num_joints);
}
else
{
// We put a segment in between each pair of points
segments_.resize(trajectory.size()-1);
// Setup up the segments
for (size_t p = 0; p < segments_.size(); ++p)
{
// This segment is from p to p+1
segments_[p].start_time = trajectory.points[p].time;
segments_[p].end_time = trajectory.points[p+1].time;
// Set up spline
segments_[p].splines.resize(num_joints);
if (trajectory.points[p].qdd.size() == trajectory.points[p].q.size())
{
result.q.resize(num_joints);
result.qd.resize(num_joints);
result.qdd.resize(num_joints);
// Have accelerations, will use Quintic.
for (size_t j = 0; j < num_joints; ++j)
{
QuinticSpline(trajectory.points[p].q[j],
trajectory.points[p].qd[j],
trajectory.points[p].qdd[j],
trajectory.points[p+1].q[j],
trajectory.points[p+1].qd[j],
trajectory.points[p+1].qdd[j],
segments_[p].end_time - segments_[p].start_time,
segments_[p].splines[j]);
}
segments_[p].type = QUINTIC;
}
else if (trajectory.points[p].qd.size() == trajectory.points[p].q.size())
{
result.q.resize(num_joints);
result.qd.resize(num_joints);
// Velocities + Positions, do Cubic.
for (size_t j = 0; j < num_joints; ++j)
{
CubicSpline(trajectory.points[p].q[j],
trajectory.points[p].qd[j],
trajectory.points[p+1].q[j],
trajectory.points[p+1].qd[j],
segments_[p].end_time - segments_[p].start_time,
segments_[p].splines[j]);
}
segments_[p].type = CUBIC;
}
else
{
result.q.resize(num_joints);
// Lame -- only positions do linear
for (size_t j = 0; j < num_joints; ++j)
{
LinearSpline(trajectory.points[p].q[j],
trajectory.points[p+1].q[j],
segments_[p].end_time - segments_[p].start_time,
segments_[p].splines[j]);
}
segments_[p].type = LINEAR;
}
}
}
seg_ = -1;
}
virtual TrajectoryPoint sample(double time)
{
// Which segment to sample from.
while ((seg_ + 1 < static_cast<int>(segments_.size())) &&
(segments_[seg_ + 1].start_time < time))
{
++seg_;
}
// Check beginning of trajectory, return empty trajectory point if not started.
if (seg_ == -1)
return TrajectoryPoint();
// Check end of trajectory, restrict time.
if (time > end_time())
time = end_time();
// Sample segment for each joint.
for (size_t i = 0; i < segments_[seg_].splines.size(); ++i)
{
if (segments_[seg_].type == QUINTIC)
{
sampleQuinticSpline(segments_[seg_].splines[i], time - segments_[seg_].start_time,
result.q[i], result.qd[i], result.qdd[i]);
}
else if(segments_[seg_].type == CUBIC)
{
sampleCubicSpline(segments_[seg_].splines[i], time - segments_[seg_].start_time,
result.q[i], result.qd[i]);
}
else
{
sampleLinearSpline(segments_[seg_].splines[i], time - segments_[seg_].start_time,
result.q[i]);
}
}
result.time = time;
return result;
}
virtual double end_time()
{
return segments_[segments_.size()-1].end_time;
}
virtual Trajectory getTrajectory()
{
return trajectory_;
}
private:
std::vector<Segment> segments_;
Trajectory trajectory_;
TrajectoryPoint result;
int seg_;
};
} // namespace robot_controllers
#endif // ROBOT_CONTROLLERS__TRAJECTORY_SPLINE_SAMPLER_H_