Commit f44c6808 authored by Steve Tonneau's avatar Steve Tonneau

added waypoints at zero velocity in quaternion interpolation. Added copy...

added waypoints at zero velocity in quaternion interpolation. Added copy constructors for exact_cubic and curve_splines. template type for exact_cubic primitive splines
parent 99a7295b
......@@ -36,7 +36,8 @@ namespace spline
/// crossing each of the waypoint given in its initialization
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
, typename Point= Eigen::Matrix<Numeric, Dim, 1>, typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> > >
, typename Point= Eigen::Matrix<Numeric, Dim, 1>, typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> >
, typename SplineBase=spline_curve<Time, Numeric, Dim, Safe, Point, T_Point> >
struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef Point point_t;
......@@ -44,7 +45,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
typedef Time time_t;
typedef Numeric num_t;
typedef spline_curve<time_t, Numeric, Dim, Safe, point_t, t_point_t> spline_t;
typedef SplineBase spline_t;
typedef typename std::vector<spline_t> t_spline_t;
typedef typename t_spline_t::iterator it_spline_t;
typedef typename t_spline_t::const_iterator cit_spline_t;
......@@ -149,7 +150,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
}
private:
exact_cubic& operator=(const exact_cubic&);
//exact_cubic& operator=(const exact_cubic&);
/* Constructors - destructors */
/*Operations*/
......@@ -193,7 +194,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Attributes*/
public:
const t_spline_t subSplines_;
t_spline_t subSplines_; // const
/*Attributes*/
};
}
......
......@@ -21,6 +21,7 @@
#define _CLASS_EFFECTOR_SPLINE_ROTATION
#include "spline/helpers/effector_spline.h"
#include "spline/curve_abc.h"
#include <Eigen/Geometry>
namespace spline
......@@ -32,19 +33,51 @@ 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;
typedef curve_abc<Time, Numeric, 4, false, quat_t> curve_abc_quat_t;
typedef std::pair<Numeric, quat_t > waypoint_quat_t;
typedef std::vector<waypoint_quat_t> t_waypoint_quat_t;
typedef Eigen::Matrix<Numeric, 1, 1> point_one_dim_t;
typedef spline_deriv_constraint <Numeric, Numeric, 1, false, point_one_dim_t> spline_deriv_constraint_one_dim;
typedef std::pair<Numeric, point_one_dim_t > waypoint_one_dim_t;
typedef std::vector<waypoint_one_dim_t> t_waypoint_one_dim_t;
class time_reparametrization_spline: public spline_deriv_constraint_one_dim
class rotation_spline: public curve_abc_quat_t
{
public:
time_reparametrization_spline()
: spline_deriv_constraint_one_dim(computeWayPoints()){}
rotation_spline (quat_ref_const_t quat_from=quat_t(0,0,0,1), quat_ref_const_t quat_to=quat_t(0,0,0,1),
const double min = 0., const double max = 1.)
: curve_abc_quat_t()
, quat_from_(quat_from.data()), quat_to_(quat_to.data()), min_(min), max_(max)
, time_reparam_(computeWayPoints())
{}
~time_reparametrization_spline(){}
~rotation_spline() {}
/* Copy Constructors / operator=*/
rotation_spline& operator=(const rotation_spline& from)
{
quat_from_ = from.quat_from_;
quat_to_ = from.quat_to_;
min_ =from.min_; max_ = from.max_;
time_reparam_ = spline_deriv_constraint_one_dim(from.time_reparam_);
}
/* Copy Constructors / operator=*/
quat_t operator()(const Numeric t) const
{
if(t<=min()) return quat_from_.coeffs();
if(t>=max()) return quat_to_.coeffs();
//normalize u
Numeric u = (t - min()) /(max() - min());
// reparametrize u
return quat_from_.slerp(time_reparam_(u)[0], quat_to_).coeffs();
}
virtual quat_t derivate(time_t /*t*/, std::size_t /*order*/) const
{
throw std::runtime_error("TODO quaternion spline does not implement derivate");
}
spline_deriv_constraint_one_dim computeWayPoints() const
{
......@@ -54,8 +87,22 @@ class time_reparametrization_spline: public spline_deriv_constraint_one_dim
waypoints.push_back(std::make_pair(1,point_one_dim_t::Ones()));
return spline_deriv_constraint_one_dim(waypoints.begin(), waypoints.end());
}
virtual time_t min() const{return min_;}
virtual time_t max() const{return max_;}
public:
Eigen::Quaterniond quat_from_; //const
Eigen::Quaterniond quat_to_; //const
double min_; //const
double max_; //const
spline_deriv_constraint_one_dim time_reparam_; //const
};
typedef exact_cubic<Time, Numeric, 4, false, quat_t, std::vector<quat_t,Eigen::aligned_allocator<quat_t> >, rotation_spline> exact_cubic_quat_t;
/// \class effector_spline_rotation
/// \brief Represents a trajectory for and end effector
/// uses the method effector_spline to create a spline trajectory.
......@@ -93,7 +140,40 @@ class effector_spline_rotation
, 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)
, rotation_spline_()
, quat_spline_(simple_quat_spline())
{
// NOTHING
}
/// \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 quatWayPointsBegin : en iterator pointing to the first element of a 4D vector (x, y, z, w) container of
/// quaternions indicating rotation at specific time steps.
/// \param quatWayPointsEnd : en iterator pointing to the end of a 4D vector (x, y, z, w) container of
/// quaternions indicating rotation at specific time steps.
/// \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, typename InQuat>
effector_spline_rotation(In wayPointsBegin, In wayPointsEnd,
InQuat quatWayPointsBegin, InQuat quatWayPointsEnd,
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_((quatWayPointsBegin->second).data()), land_quat_(((quatWayPointsEnd-1)->second).data())
, time_lift_offset_(spline_->min()+lift_offset_duration)
, time_land_offset_(spline_->max()-land_offset_duration)
, quat_spline_(quat_spline(quatWayPointsBegin, quatWayPointsEnd))
{
// NOTHING
}
......@@ -122,19 +202,39 @@ class effector_spline_rotation
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(Numeric t) const
public:
quat_t interpolate_quat(const Numeric t) const
{
if(t<=time_lift_offset_) return to_quat_.coeffs();
if(t>=time_land_offset_) return land_quat_.coeffs();
//normalize u
Numeric u = (t - time_lift_offset_) /(time_land_offset_ - time_lift_offset_);
// reparametrize u
return to_quat_.slerp(rotation_spline_(u)[0], land_quat_).coeffs();
return quat_spline_(t);
}
private:
exact_cubic_quat_t simple_quat_spline() const
{
std::vector<rotation_spline> splines;
splines.push_back(rotation_spline(to_quat_.coeffs(),land_quat_.coeffs(),time_lift_offset_, time_land_offset_));
return exact_cubic_quat_t(splines);
}
template <typename InQuat>
exact_cubic_quat_t quat_spline(InQuat quatWayPointsBegin, InQuat quatWayPointsEnd) const
{
if(std::distance(quatWayPointsBegin, quatWayPointsEnd) < 1)
return simple_quat_spline();
exact_cubic_quat_t::t_spline_t splines;
InQuat it(quatWayPointsBegin);
Time init = time_lift_offset_;
Eigen::Quaterniond current_quat = to_quat_;
for(; it != quatWayPointsEnd; ++it)
{
splines.push_back(rotation_spline(current_quat.coeffs(), it->second, init, it->first));
current_quat = it->second;
init = it->first;
}
splines.push_back(rotation_spline(current_quat.coeffs(), land_quat_.coeffs(), init, time_land_offset_));
return exact_cubic_quat_t(splines);
}
/*Operations*/
......@@ -146,7 +246,7 @@ class effector_spline_rotation
const Eigen::Quaterniond land_quat_;
const double time_lift_offset_;
const double time_land_offset_;
const time_reparametrization_spline rotation_spline_; // should be static
const exact_cubic_quat_t quat_spline_;
/*Attributes*/
};
......
......@@ -38,8 +38,9 @@ namespace spline
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
typename Point= Eigen::Matrix<Numeric, Dim, 1>,
typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> > >
struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point>
typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> >,
typename SplineBase=spline_curve<Time, Numeric, Dim, Safe, Point, T_Point> >
struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase>
{
typedef Point point_t;
typedef T_Point t_point_t;
......@@ -83,7 +84,6 @@ struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Po
spline_deriv_constraint(const spline_deriv_constraint& other)
: exact_cubic_t(other.subSplines_) {}
private:
template<typename In>
void compute_one_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines) const
......@@ -146,7 +146,6 @@ struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Po
}
private:
spline_deriv_constraint& operator=(const spline_deriv_constraint&);
/* Constructors - destructors */
};
}
......
......@@ -429,6 +429,13 @@ helpers::quat_t GetXRotQuat(const double theta)
return helpers::quat_t(Eigen::Quaterniond(m).coeffs().data());
}
double GetXRotFromQuat(helpers::quat_ref_const_t q)
{
Eigen::Quaterniond quat (q.data());
Eigen::AngleAxisd m (quat);
return m.angle() / M_PI * 180.;
}
void EffectorSplineRotationNoRotationTest(bool& error)
{
// create arbitrary trajectory
......@@ -466,7 +473,7 @@ void EffectorSplineRotationRotationTest(bool& error)
helpers::config_t q_to = q_init; q_to(2) +=0.02;
helpers::config_t q_land = q_end ; q_land(2)+=0.02;
helpers::quat_t q_mod = GetXRotQuat(M_PI_2);;
std::string errmsg("Error in EffectorSplineRotationNoRotationTest; while checking waypoints (expected / obtained)");
std::string errmsg("Error in EffectorSplineRotationRotationTest; 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);
......@@ -474,9 +481,46 @@ void EffectorSplineRotationRotationTest(bool& error)
ComparePoints(q_end , eff_traj(10), errmsg,error);
}
void EffectorSplineRotationWayPointRotationTest(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::quat_t init_quat = GetXRotQuat(0);
helpers::t_waypoint_quat_t quat_waypoints_;
helpers::quat_t q_pi_0 = GetXRotQuat(0);
helpers::quat_t q_pi_2 = GetXRotQuat(M_PI_2);
helpers::quat_t q_pi = GetXRotQuat(M_PI);
quat_waypoints_.push_back(std::make_pair(0.4,q_pi_0));
quat_waypoints_.push_back(std::make_pair(6,q_pi_2));
quat_waypoints_.push_back(std::make_pair(8,q_pi));
helpers::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end(),
quat_waypoints_.begin(), quat_waypoints_.end());
helpers::config_t q_init = helpers::config_t::Zero(); q_init.tail<4>() = init_quat;
helpers::config_t q_end; q_end << 10.,10.,10.,0.,0.,0.,1.; q_end.tail<4>() = q_pi;
helpers::config_t q_mod; q_mod.head<3>() = point_t(6,6,6) ; q_mod.tail<4>() = q_pi_2;
helpers::config_t q_to = q_init; q_to(2) +=0.02;
helpers::config_t q_land = q_end ; q_land(2)+=0.02;
std::string errmsg("Error in EffectorSplineRotationWayPointRotationTest; 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);
}
void TestReparametrization(bool& error)
{
helpers::time_reparametrization_spline sp;
helpers::rotation_spline s;
const helpers::spline_deriv_constraint_one_dim& sp = s.time_reparam_;
if(sp.min() != 0)
{
std::cout << "in TestReparametrization; min value is not 0, got " << sp.min() << std::endl;
......@@ -521,6 +565,7 @@ int main(int /*argc*/, char** /*argv[]*/)
EffectorSplineRotationNoRotationTest(error);
EffectorSplineRotationRotationTest(error);
TestReparametrization(error);
EffectorSplineRotationWayPointRotationTest(error);
BezierCurveTest(error);
if(error)
{
......
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