Commit c0022298 authored by Steve Tonneau's avatar Steve Tonneau

effector spline helper with rotation. tests not all written

parent 5ead2d60
......@@ -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))
......
......@@ -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*/
......
......@@ -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.
......
......@@ -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);
......
/**
* \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
......@@ -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_);
......
......@@ -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)
{
......
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