From c0022298cc61164a288c70904cee3f30ecaf91ac Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@axle.laas.fr>
Date: Thu, 26 Jan 2017 13:14:49 +0100
Subject: [PATCH] effector spline helper with rotation. tests not all written
---
include/spline/bezier_curve.h | 2 +-
include/spline/curve_abc.h | 2 +-
include/spline/exact_cubic.h | 6 +-
include/spline/helpers/effector_spline.h | 16 +--
.../spline/helpers/effector_spline_rotation.h | 127 ++++++++++++++++++
include/spline/spline_curve.h | 2 +-
src/tests/spline_test/Main.cpp | 55 +++++---
7 files changed, 176 insertions(+), 34 deletions(-)
create mode 100644 include/spline/helpers/effector_spline_rotation.h
diff --git a/include/spline/bezier_curve.h b/include/spline/bezier_curve.h
index afac9bc..6343de6 100644
--- a/include/spline/bezier_curve.h
+++ b/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))
diff --git a/include/spline/curve_abc.h b/include/spline/curve_abc.h
index 0eb17f0..86f7519 100644
--- a/include/spline/curve_abc.h
+++ b/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*/
diff --git a/include/spline/exact_cubic.h b/include/spline/exact_cubic.h
index b232fd9..5f50717 100644
--- a/include/spline/exact_cubic.h
+++ b/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.
diff --git a/include/spline/helpers/effector_spline.h b/include/spline/helpers/effector_spline.h
index 93007fb..9928412 100644
--- a/include/spline/helpers/effector_spline.h
+++ b/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);
diff --git a/include/spline/helpers/effector_spline_rotation.h b/include/spline/helpers/effector_spline_rotation.h
new file mode 100644
index 0000000..d778d0e
--- /dev/null
+++ b/include/spline/helpers/effector_spline_rotation.h
@@ -0,0 +1,127 @@
+/**
+* \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
+
diff --git a/include/spline/spline_curve.h b/include/spline/spline_curve.h
index 5e12363..07f8248 100644
--- a/include/spline/spline_curve.h
+++ b/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_);
diff --git a/src/tests/spline_test/Main.cpp b/src/tests/spline_test/Main.cpp
index bcb0688..c07187a 100644
--- a/src/tests/spline_test/Main.cpp
+++ b/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)
{
--
GitLab