diff --git a/include/spline/exact_cubic.h b/include/spline/exact_cubic.h
index 8cb96c459529b1b2fee5050ce241c5972635eb91..b232fd989825cc49f6e8f99b6cea331a5a53f4b5 100644
--- a/include/spline/exact_cubic.h
+++ b/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
{
diff --git a/include/spline/helpers/effector_spline.h b/include/spline/helpers/effector_spline.h
new file mode 100644
index 0000000000000000000000000000000000000000..93007fb4cac1bc7e2fb121282d995ff9c9978b2e
--- /dev/null
+++ b/include/spline/helpers/effector_spline.h
@@ -0,0 +1,123 @@
+/**
+* \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
+
diff --git a/include/spline/spline_deriv_constraint.h b/include/spline/spline_deriv_constraint.h
index 9ab09eaa129a2bea1ed2a38eb0d165c2be911773..28cb05e903ce51e9149793073fda1be223045378 100644
--- a/include/spline/spline_deriv_constraint.h
+++ b/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
diff --git a/src/tests/spline_test/Main.cpp b/src/tests/spline_test/Main.cpp
index f236a2a2f672fca54e606194fc1ac3e94499ade5..bcb0688b99a2f02814474859a1184858bee583d7 100644
--- a/src/tests/spline_test/Main.cpp
+++ b/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