Commit 5ead2d60 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

helper method for effector splines

parent 0a87d54b
......@@ -152,7 +152,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param t : the time when to evaluate the spline
/// \param return : the value x(t)
virtual point_t operator()(time_t t) const
{
......@@ -167,7 +167,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
}
/// \brief Evaluation of the derivative spline at time t.
/// \param t : the time when to evaluate the spine
/// \param t : the time when to evaluate the spline
/// \param return : the value x(t)
virtual point_t derivate(time_t t, std::size_t order) const
{
......
/**
* \file exact_cubic.h
* \brief class allowing to create an Exact cubic spline.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* This file contains definitions for the ExactCubic class.
* Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique set of
* cubic splines fulfulling those 4 restrictions :
* - x_i(t_i) = x_i* ; this means that the curve passes trough each waypoint
* - x_i(t_i+1) = x_i+1* ;
* - its derivative is continous at t_i+1
* - its acceleration is continous at t_i+1
* more details in paper "Task-Space Trajectories via Cubic Spline Optimization"
* By J. Zico Kolter and Andrew Y.ng (ICRA 2009)
*/
#ifndef _CLASS_EFFECTORSPLINE
#define _CLASS_EFFECTORSPLINE
#include "spline/spline_deriv_constraint.h"
namespace spline
{
namespace helpers
{
typedef double Numeric;
typedef double Time;
typedef Eigen::Matrix<Numeric, 3, 1> Point;
typedef std::vector<Point,Eigen::aligned_allocator<Point> > T_Point;
typedef std::pair<double, Point> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
typedef spline_deriv_constraint<Time, Numeric, 3, true, Point, T_Point> spline_deriv_constraint_t;
typedef spline_deriv_constraint_t::spline_constraints spline_constraints_t;
typedef spline_deriv_constraint_t::exact_cubic_t exact_cubic_t;
typedef spline_deriv_constraint_t::t_spline_t t_spline_t;
typedef spline_deriv_constraint_t::spline_t spline_t;
Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numeric offset, const Time time_offset)
{
//compute time such that the equation from source to offsetpoint is necessarily a line
Numeric norm = normal.norm();
assert(norm>0.);
return std::make_pair(source.first + time_offset,(source.second + normal / norm* offset));
}
spline_t make_end_spline(const Point& normal, const Point& from, const Numeric offset,
const Time init_time, const Time time_offset)
{
// compute spline from land way point to end point
// constraints are null velocity and acceleration
Numeric norm = normal.norm();
assert(norm>0.);
Point n = normal / norm;
Point d = offset / (time_offset*time_offset*time_offset) * -n;
Point c = -3 * d * time_offset;
Point b = -c * time_offset;
T_Point points;
points.push_back(from);
points.push_back(b);
points.push_back(c);
points.push_back(d);
return spline_t(points.begin(), points.end(), init_time, init_time+time_offset);
}
spline_constraints_t compute_required_offset_velocity_acceleration(const spline_t& end_spline, const Time time_offset)
{
//computing end velocity: along landing normal and respecting time
spline_constraints_t constraints;
constraints.end_acc = end_spline.derivate(end_spline.min(),2);
constraints.end_vel = end_spline.derivate(end_spline.min(),1);
return constraints;
}
/// \brief Helper method to create a spline typically used to
/// guide the 2d or 3d trajectory of a robot end effector.
/// Given a set of waypoints, and the normal vector of the start and
/// ending positions, automatically create the spline such that:
/// + init and end velocities / accelerations are 0.
/// + the effector lifts and lands exactly in the direction of the specified normals
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container
/// \param wayPointsEnd : an iterator pointing to the end of a waypoint container
/// \param lift_normal : normal to be followed by end effector at take-off
/// \param land_normal : normal to be followed by end effector at landing
/// \param lift_offset : length of the straight line along normal at take-off
/// \param land_offset : length of the straight line along normal at landing
/// \param time_lift_offset : time travelled along straight line at take-off
/// \param time_land_offset : time travelled along straight line at landing
///
template<typename In>
exact_cubic_t* effector_spline(
In wayPointsBegin, In wayPointsEnd, const Point& lift_normal=Eigen::Vector3d::UnitZ(), const Point& land_normal=Eigen::Vector3d::UnitZ(),
const Numeric lift_offset=0.02, const Numeric land_offset=0.02,
const Time time_lift_offset=0.02, const Time time_land_offset=0.02)
{
T_Waypoint waypoints;
const Waypoint& inPoint=*wayPointsBegin, endPoint =*(wayPointsEnd-1);
waypoints.push_back(inPoint);
//adding initial offset
waypoints.push_back(compute_offset(inPoint, lift_normal ,lift_offset, time_lift_offset));
//inserting all waypoints but last
waypoints.insert(waypoints.end(),wayPointsBegin+1,wayPointsEnd-1);
//inserting waypoint to start landing
const Waypoint& landWaypoint=compute_offset(endPoint, land_normal ,land_offset, -time_land_offset);
waypoints.push_back(landWaypoint);
//specifying end velocity constraint such that landing will be in straight line
spline_t end_spline=make_end_spline(land_normal,landWaypoint.second,land_offset,landWaypoint.first,time_land_offset);
spline_constraints_t constraints = compute_required_offset_velocity_acceleration(end_spline,time_land_offset);
spline_deriv_constraint_t all_but_end(waypoints.begin(), waypoints.end(),constraints);
t_spline_t splines = all_but_end.subSplines_;
splines.push_back(end_spline);
return new exact_cubic_t(splines);
}
}
}
#endif //_CLASS_EFFECTORSPLINE
......@@ -70,7 +70,8 @@ struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Po
public:
///\brief Constructor
///\param wayPointsBegin : an iterator pointing to the first element of a waypoint container
///\param wayPointsEns : an iterator pointing to the end of a waypoint container
///\param wayPointsEnd : an iterator pointing to the end of a waypoint container
///\param constraints : constraints on the init and end velocity / accelerations of the spline
template<typename In>
spline_deriv_constraint(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints = spline_constraints())
: exact_cubic_t(computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints)) {}
......@@ -143,10 +144,6 @@ struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Po
spline_deriv_constraint(const spline_deriv_constraint&);
spline_deriv_constraint& operator=(const spline_deriv_constraint&);
/* Constructors - destructors */
/*Attributes*/
public:
const t_spline_t subSplines_;
/*Attributes*/
};
}
#endif //_CLASS_CUBICZEROVELACC
......
......@@ -3,6 +3,7 @@
#include "spline/bezier_curve.h"
#include "spline/spline_curve.h"
#include "spline/spline_deriv_constraint.h"
#include "spline/helpers/effector_spline.h"
#include <string>
#include <iostream>
......@@ -378,6 +379,63 @@ void ExactCubicVelocityConstraintsTest(bool& error)
CheckDerivative(errmsg4,1,2,constraints.end_acc ,&exactCubic2, error);
}
void CheckPointOnline(const std::string& errmsg, const point_t& A, const point_t& B, const double target, const exact_cubic_t* curve, bool& error )
{
point_t res1 = curve->operator ()(target);
point_t ar =(res1-A); ar.normalize();
point_t rb =(B-res1); rb.normalize();
if(ar.dot(rb) < 0.99999)
{
error = true;
std::cout << errmsg << " ; " << A.transpose() << "\n ; " << B.transpose() << "\n ; " <<
target << " ; " << res1.transpose() << std::endl;
}
}
void EffectorTrajectoryTest(bool& error)
{
// create arbitrary trajectory
spline::T_Waypoint waypoints;
for(double i = 0; i <= 10; i = i + 2)
{
waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
}
helpers::exact_cubic_t* eff_traj = helpers::effector_spline(waypoints.begin(),waypoints.end(),
Eigen::Vector3d::UnitZ(),Eigen::Vector3d(0,0,2),
1,1,1,0.5);
point_t zero(0,0,0);
point_t off1(0,0,1);
point_t off2(10,10,11);
point_t end(10,10,10);
std::string errmsg("Error in EffectorTrajectoryTest; while checking waypoints (expected / obtained)");
std::string errmsg2("Error in EffectorTrajectoryTest; while checking derivative (expected / obtained)");
//first check start / goal positions
ComparePoints(zero, (*eff_traj)(0), errmsg, error);
ComparePoints(off1, (*eff_traj)(1), errmsg, error);
ComparePoints(off2, (*eff_traj)(9.5), errmsg, error);
ComparePoints(end , (*eff_traj)(10), errmsg, error);
//then check offset at start / goal positions
// now check derivatives
CheckDerivative(errmsg2,0,1,zero,eff_traj, error);
CheckDerivative(errmsg2,10,1,zero ,eff_traj, error);
CheckDerivative(errmsg2,0,2,zero,eff_traj, error);
CheckDerivative(errmsg2,10,2,zero ,eff_traj, error);
//check that end and init splines are line
std::string errmsg3("Error in EffectorTrajectoryTest; while checking that init/end splines are line (point A/ point B, time value / point obtained) \n");
for(double i = 0.1; i<1; i+=0.1)
{
CheckPointOnline(errmsg3,(*eff_traj)(0),(*eff_traj)(1),i,eff_traj,error);
}
for(double i = 9.6; i<10; i+=0.1)
{
CheckPointOnline(errmsg3,(*eff_traj)(9.5),(*eff_traj)(10),i,eff_traj,error);
}
}
int main(int /*argc*/, char** /*argv[]*/)
{
......@@ -389,10 +447,11 @@ int main(int /*argc*/, char** /*argv[]*/)
ExactCubicTwoPointsTest(error);
ExactCubicOneDimTest(error);
ExactCubicVelocityConstraintsTest(error);
EffectorTrajectoryTest(error);
//BezierCurveTest(error);
if(error)
{
std::cout << "There were some errors\n";
std::cout << "There were some errors\n";
return -1;
}
else
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment