Commit 0c4a333d authored by Steve Tonneau's avatar Steve Tonneau

zero vel/acc also for rotations + added copy constructors for spline to avoid pointers

parent 5cba1d27
......@@ -31,10 +31,10 @@ struct curve_abc : std::unary_function<Time, Point>
/* Constructors - destructors */
public:
///\brief Constructor
curve_abc(){};
curve_abc(){}
///\brief Destructor
~curve_abc(){};
~curve_abc(){}
/* Constructors - destructors */
/*Operations*/
......
......@@ -65,6 +65,10 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
exact_cubic(const t_spline_t& subSplines)
: curve_abc_t(), subSplines_(subSplines) {}
///\brief Copy Constructor
exact_cubic(const exact_cubic& other)
: curve_abc_t(), subSplines_(other.subSplines_) {}
///\brief Destructor
~exact_cubic(){}
......@@ -144,8 +148,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
return subSplines;
}
private:
exact_cubic(const exact_cubic&);
private:
exact_cubic& operator=(const exact_cubic&);
/* Constructors - destructors */
......
......@@ -27,10 +27,34 @@ 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;
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
{
public:
time_reparametrization_spline()
: spline_deriv_constraint_one_dim(computeWayPoints()){}
~time_reparametrization_spline(){}
spline_deriv_constraint_one_dim computeWayPoints() const
{
// initializing time reparametrization for spline
t_waypoint_one_dim_t waypoints;
waypoints.push_back(std::make_pair(0,point_one_dim_t::Zero()));
waypoints.push_back(std::make_pair(1,point_one_dim_t::Ones()));
return spline_deriv_constraint_one_dim(waypoints.begin(), waypoints.end());
}
};
/// \class effector_spline_rotation
/// \brief Represents a trajectory for and end effector
......@@ -69,6 +93,7 @@ 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_()
{
// NOTHING
}
......@@ -97,6 +122,7 @@ 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
......@@ -107,8 +133,10 @@ class effector_spline_rotation
if(t>=time_land_offset_) return land_quat_.coeffs();
//normalize u
Numeric u = (t - time_lift_offset_) /(time_land_offset_ - time_lift_offset_);
return to_quat_.slerp(u, land_quat_).coeffs();
// reparametrize u
return to_quat_.slerp(rotation_spline_(u)[0], land_quat_).coeffs();
}
/*Operations*/
/*Attributes*/
......@@ -118,6 +146,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
/*Attributes*/
};
......
......@@ -95,8 +95,10 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
: coefficients_(other.coefficients_), dim_(other.dim_), order_(other.order_),
t_min_(other.t_min_), t_max_(other.t_max_){}
private:
//spline_curve& operator=(const spline_curve& other);
private:
void safe_check()
{
if(Safe)
......@@ -162,9 +164,9 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Attributes*/
public:
coeff_t coefficients_;
std::size_t dim_;
std::size_t order_;
coeff_t coefficients_; //const
std::size_t dim_; //const
std::size_t order_; //const
private:
time_t t_min_, t_max_;
......
......@@ -79,6 +79,11 @@ struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Po
///\brief Destructor
~spline_deriv_constraint(){}
///\brief Copy Constructor
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
......@@ -140,8 +145,7 @@ struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Po
return subSplines;
}
private:
spline_deriv_constraint(const spline_deriv_constraint&);
private:
spline_deriv_constraint& operator=(const spline_deriv_constraint&);
/* Constructors - destructors */
};
......
......@@ -474,6 +474,39 @@ void EffectorSplineRotationRotationTest(bool& error)
ComparePoints(q_end , eff_traj(10), errmsg,error);
}
void TestReparametrization(bool& error)
{
helpers::time_reparametrization_spline sp;
if(sp.min() != 0)
{
std::cout << "in TestReparametrization; min value is not 0, got " << sp.min() << std::endl;
error = true;
}
if(sp.max() != 1)
{
std::cout << "in TestReparametrization; max value is not 1, got " << sp.max() << std::endl;
error = true;
}
if(sp(1)[0] != 1.)
{
std::cout << "in TestReparametrization; end value is not 1, got " << sp(1)[0] << std::endl;
error = true;
}
if(sp(0)[0] != 0.)
{
std::cout << "in TestReparametrization; init value is not 0, got " << sp(0)[0] << std::endl;
error = true;
}
for(double i =0; i<1; i+=0.002)
{
if(sp(i)[0]>sp(i+0.002)[0])
{
std::cout << "in TestReparametrization; reparametrization not monotonous " << sp.max() << std::endl;
error = true;
}
}
}
int main(int /*argc*/, char** /*argv[]*/)
{
std::cout << "performing tests... \n";
......@@ -487,6 +520,7 @@ int main(int /*argc*/, char** /*argv[]*/)
EffectorTrajectoryTest(error);
EffectorSplineRotationNoRotationTest(error);
EffectorSplineRotationRotationTest(error);
TestReparametrization(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