helper method for effector splines

include/spline/exact_cubic.h

@@ -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
     {

include/spline/helpers/effector_spline.h

+/**
+* \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
+

include/spline/spline_deriv_constraint.h

@@ -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

src/tests/spline_test/Main.cpp

@@ -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