effector spline helper with rotation. tests not all written

include/spline/bezier_curve.h

@@ -67,7 +67,7 @@ struct bezier_curve : public  curve_abc<Time, Numeric, Dim, Safe, Point>
 	///  \brief Evaluation of the cubic spline at time t.
 	///  \param t : the time when to evaluate the spine
 	///  \param return : the value x(t)
-	virtual point_t operator()(time_t t) const
+    virtual point_t operator()(const time_t t) const
 	{
 		num_t nT = (t - minBound_) / (maxBound_ - minBound_);
 		if(Safe &! (0 <= nT && nT <= 1))

include/spline/curve_abc.h

@@ -42,7 +42,7 @@ struct  curve_abc : std::unary_function<Time, Point>
 	///  \brief Evaluation of the cubic spline at time t.
 	///  \param t : the time when to evaluate the spine
 	///  \param return : the value x(t)
-    virtual point_t operator()(time_t t) const = 0;
+    virtual point_t operator()(const time_t t) const = 0;
 /*Operations*/
 
 /*Helpers*/

include/spline/exact_cubic.h

@@ -154,16 +154,16 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
 	///  \brief Evaluation of the cubic spline at time t.
     ///  \param t : the time when to evaluate the spline
 	///  \param return : the value x(t)
-	virtual point_t operator()(time_t t) const
+    virtual point_t operator()(const time_t t) const
     {
         if(Safe && (t < subSplines_.front().min() || t > subSplines_.back().max())){throw std::out_of_range("TODO");}
         for(cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++ it)
 		{
             if(t >= (it->min()) && t <= (it->max()))
-			{
+            {
                 return it->operator()(t);
 			}
-		}
+        }
     }
 
     ///  \brief Evaluation of the derivative spline at time t.

include/spline/helpers/effector_spline.h

@@ -79,7 +79,7 @@ spline_constraints_t compute_required_offset_velocity_acceleration(const spline_
 
 
 /// \brief Helper method to create a spline typically used to
-/// guide the 2d or 3d trajectory of a robot end effector.
+/// guide the 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.
@@ -90,28 +90,28 @@ spline_constraints_t compute_required_offset_velocity_acceleration(const spline_
 /// \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
+/// \param lift_offset_duration : time travelled along straight line at take-off
+/// \param land_offset_duration : 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)
+        const Time lift_offset_duration=0.02, const Time land_offset_duration=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));
+    waypoints.push_back(compute_offset(inPoint, lift_normal ,lift_offset, lift_offset_duration));
     //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);
+    const Waypoint& landWaypoint=compute_offset(endPoint, land_normal ,land_offset, -land_offset_duration);
     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_t end_spline=make_end_spline(land_normal,landWaypoint.second,land_offset,landWaypoint.first,land_offset_duration);
+    spline_constraints_t constraints = compute_required_offset_velocity_acceleration(end_spline,land_offset_duration);
     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);

include/spline/helpers/effector_spline_rotation.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_EFFECTOR_SPLINE_ROTATION
+#define _CLASS_EFFECTOR_SPLINE_ROTATION
+
+#include "spline/helpers/effector_spline.h"
+#include <Eigen/Geometry>
+
+namespace spline
+{
+namespace helpers
+{
+typedef Eigen::Matrix<Numeric, 4, 1> quat_t;
+typedef Eigen::Ref<quat_t> quat_ref_t;
+typedef const Eigen::Ref<const quat_t> quat_ref_const_t;
+typedef Eigen::Matrix<Numeric, 7, 1> config_t;
+
+/// \class effector_spline_rotation
+/// \brief Represents a trajectory for and end effector
+/// uses the method effector_spline to create a spline trajectory.
+/// Additionally, handles the rotation of the effector as follows:
+/// does not rotate during the take off and landing phase,
+/// then uses a SLERP algorithm to interpolate the rotation in the quaternion
+/// space.
+class effector_spline_rotation
+{
+    /* Constructors - destructors */
+    public:
+    /// \brief Constructor.
+    /// 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 to_quat          : 4D vector, quaternion indicating rotation at take off(x, y, z, w)
+    /// \param land_quat        : 4D vector, quaternion indicating rotation at landing (x, y, z, w)
+    /// \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 lift_offset_duration : time travelled along straight line at take-off
+    /// \param land_offset_duration : time travelled along straight line at landing
+    ///
+    template<typename In>
+    effector_spline_rotation(In wayPointsBegin, In wayPointsEnd,
+            quat_ref_const_t& to_quat=quat_t(0,0,0,1), quat_ref_const_t& land_quat=quat_t(0,0,0,1),
+            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 lift_offset_duration=0.02, const Time land_offset_duration=0.02)
+        : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, land_normal, lift_offset, land_offset, lift_offset_duration, land_offset_duration))
+        , to_quat_(to_quat.data()), land_quat_(land_quat.data())
+        , time_lift_offset_(spline_->min()+lift_offset_duration)
+        , time_land_offset_(spline_->max()-land_offset_duration)
+    {
+        // NOTHING
+    }
+
+    ~effector_spline_rotation() {delete spline_;}
+    /* Constructors - destructors */
+
+    /*Helpers*/
+    public:
+    Numeric min() const{return spline_->min();}
+    Numeric max() const{return spline_->max();}
+    /*Helpers*/
+
+    /*Operations*/
+    public:
+    ///  \brief Evaluation of the effector position and rotation at time t.
+    ///  \param t : the time when to evaluate the spline
+    ///  \param return : a 7D vector; The 3 first values are the 3D position, the 4 last the
+    ///  quaternion describing the rotation
+    ///
+    config_t operator()(const Numeric t) const
+    {
+        config_t res;
+        res.head<3>() = (*spline_)(t);
+        res.tail<4>() = interpolate_quat(t);
+        return res;
+    }
+
+    ///  \brief Interpolates between two quaternions
+    ///  \param t : the time when to evaluate the spline
+    ///  \param quat : quaternion updated as the interpolation result
+    ///
+    quat_t interpolate_quat(time_t t) const
+    {
+        if(t<=time_lift_offset_) return quat_t(to_quat_.coeffs().data());
+        if(t>=time_land_offset_) return quat_t(land_quat_.coeffs().data());
+        //normalize u
+        Numeric u = (t - time_lift_offset_) /(time_land_offset_ - time_lift_offset_);
+        return quat_t(to_quat_.slerp(u, land_quat_).coeffs().data());
+    }
+    /*Operations*/
+
+    /*Attributes*/
+    public:
+    const exact_cubic_t* spline_;
+    const Eigen::Quaterniond to_quat_;
+    const Eigen::Quaterniond land_quat_;
+    const double time_lift_offset_;
+    const double time_land_offset_;
+    /*Attributes*/
+};
+
+} // namespace helpers
+} // namespace spline
+#endif //_CLASS_EFFECTOR_SPLINE_ROTATION
+

include/spline/spline_curve.h

@@ -115,7 +115,7 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
     ///  \brief Evaluation of the cubic spline at time t.
     ///  \param t : the time when to evaluate the spine
     ///  \param return : the value x(t)
-    virtual point_t operator()(time_t t) const
+    virtual point_t operator()(const time_t t) const
     {
         if((t < t_min_ || t > t_max_) && Safe){ throw std::out_of_range("TODO");}
         time_t const dt (t-t_min_);

src/tests/spline_test/Main.cpp

@@ -4,6 +4,7 @@
 #include "spline/spline_curve.h"
 #include "spline/spline_deriv_constraint.h"
 #include "spline/helpers/effector_spline.h"
+#include "spline/helpers/effector_spline_rotation.h"
 
 #include <string>
 #include <iostream>
@@ -42,12 +43,7 @@ bool QuasiEqual(const double a, const double b, const float margin)
 	}
 }
 
-const float margin = 0.01f;
-
-bool operator ==(const point_t& a, const point_t& b)
-{
-	return QuasiEqual(a.x(), b.x(), margin) && QuasiEqual(a.y(), b.y(), margin) && QuasiEqual(a.z(), b.z(), margin);
-}
+const double margin = 0.001;
 
 } // namespace spline
 
@@ -59,24 +55,15 @@ ostream& operator<<(ostream& os, const point_t& pt)
     return os;
 }
 
-void ComparePoints(const point_t& pt1, const point_t& pt2, const std::string& errmsg, bool& error)
+void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, const std::string& errmsg, bool& error)
 {
-	if(!(pt1 == pt2))
+    if((pt1-pt2).norm() > margin)
 	{
 		error = true;
         std::cout << errmsg << pt1 << " ; " << pt2 << std::endl;
 	}
 }
 
-void ComparePoints(const point_one& pt1, const point_one& pt2, const std::string& errmsg, bool& error)
-{
-    if(!(pt1 == pt2))
-	{
-		error = true;
-        std::cout << errmsg << pt1 << " ; " << pt2 <<  std::endl;
-	}
-}
-
 /*Cubic Function tests*/
 
 void CubicFunctionTest(bool& error)
@@ -402,10 +389,10 @@ void EffectorTrajectoryTest(bool& error)
     }
     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);
+                                                               1,0.02,1,0.5);
     point_t zero(0,0,0);
     point_t off1(0,0,1);
-    point_t off2(10,10,11);
+    point_t off2(10,10,10.02);
     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)");
@@ -429,13 +416,40 @@ void EffectorTrajectoryTest(bool& error)
         CheckPointOnline(errmsg3,(*eff_traj)(0),(*eff_traj)(1),i,eff_traj,error);
     }
 
-    for(double i = 9.6; i<10; i+=0.1)
+    for(double i = 9.981; i<10; i+=0.002)
     {
         CheckPointOnline(errmsg3,(*eff_traj)(9.5),(*eff_traj)(10),i,eff_traj,error);
     }
+    delete eff_traj;
+}
 
+helpers::quat_t GetXRotQuat(const double theta)
+{
+    Eigen::AngleAxisd m (theta, Eigen::Vector3d::UnitX());
+    return helpers::quat_t(Eigen::Quaterniond(m).coeffs().data());
 }
 
+void EffectorSplineRotationNoRotationTest(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::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end());
+    helpers::config_t q_init; q_init    << 0.,0.,0.,0.,0.,0.,1.;
+    helpers::config_t q_end; q_end      << 10.,10.,10.,0.,0.,0.,1.;
+    helpers::config_t q_to; q_to        << 0.,0,0.02,0.,0.,0.,1.;
+    helpers::config_t q_land; q_land    << 10,10, 10.02, 0, 0.,0.,1.;
+    helpers::config_t q_mod; q_mod      << 6.,6.,6.,0.,0.,0.,1.;
+    std::string errmsg("Error in EffectorSplineRotationNoRotationTest; while checking waypoints (expected / obtained)");
+    ComparePoints(q_init , eff_traj(0),    errmsg,error);
+    ComparePoints(q_to   , eff_traj(0.02), errmsg,error);
+    ComparePoints(q_land , eff_traj(9.98), errmsg,error);
+    ComparePoints(q_mod  , eff_traj(6),    errmsg,error);
+    ComparePoints(q_end  , eff_traj(10),   errmsg,error);
+}
 
 int main(int /*argc*/, char** /*argv[]*/)
 {
@@ -448,6 +462,7 @@ int main(int /*argc*/, char** /*argv[]*/)
     ExactCubicOneDimTest(error);
     ExactCubicVelocityConstraintsTest(error);
     EffectorTrajectoryTest(error);
+    EffectorSplineRotationNoRotationTest(error);
     //BezierCurveTest(error);
 	if(error)
 	{