From c55a3f935e22a6db62328a7810b9b47190557109 Mon Sep 17 00:00:00 2001
From: JasonChmn <jason.chemin@hotmail.fr>
Date: Thu, 8 Aug 2019 15:24:16 +0200
Subject: [PATCH] Remove aligned_vector file / redo indentation in helpers
---
include/curves/helpers/effector_spline.h | 173 ++-
.../curves/helpers/effector_spline_rotation.h | 403 +++----
include/curves/optimization/OptimizeSpline.h | 1009 ++++++++---------
include/curves/serialization/CMakeLists.txt | 1 -
4 files changed, 792 insertions(+), 794 deletions(-)
diff --git a/include/curves/helpers/effector_spline.h b/include/curves/helpers/effector_spline.h
index 7a4a9e2..7cf61b2 100644
--- a/include/curves/helpers/effector_spline.h
+++ b/include/curves/helpers/effector_spline.h
@@ -24,97 +24,96 @@
namespace curves
{
-namespace helpers
-{
-typedef double Numeric;
-typedef double Time;
-typedef Eigen::Matrix<Numeric, 3, 1> Point;
-typedef std::vector<Point,Eigen::aligned_allocator<Point> > T_Point;
-typedef std::pair<double, Point> Waypoint;
-typedef std::vector<Waypoint> T_Waypoint;
-typedef exact_cubic<Time, Numeric, 3, true, Point, T_Point> exact_cubic_t;
-typedef exact_cubic_t::spline_constraints spline_constraints_t;
-typedef exact_cubic_t::t_spline_t t_spline_t;
-typedef exact_cubic_t::spline_t spline_t;
+ namespace helpers
+ {
+ typedef double Numeric;
+ typedef double Time;
+ typedef Eigen::Matrix<Numeric, 3, 1> Point;
+ typedef std::vector<Point,Eigen::aligned_allocator<Point> > T_Point;
+ typedef std::pair<double, Point> Waypoint;
+ typedef std::vector<Waypoint> T_Waypoint;
+ typedef exact_cubic<Time, Numeric, 3, true, Point, T_Point> exact_cubic_t;
+ typedef exact_cubic_t::spline_constraints spline_constraints_t;
+ typedef exact_cubic_t::t_spline_t t_spline_t;
+ typedef exact_cubic_t::spline_t spline_t;
-/// \brief Compute time such that the equation from source to offsetpoint is necessarily a line.
-Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numeric offset, const Time time_offset)
-{
- Numeric norm = normal.norm();
- assert(norm>0.);
- return std::make_pair(source.first + time_offset,(source.second + normal / norm* offset));
-}
+ /// \brief Compute time such that the equation from source to offsetpoint is necessarily a line.
+ Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numeric offset, const Time time_offset)
+ {
+ Numeric norm = normal.norm();
+ assert(norm>0.);
+ return std::make_pair(source.first + time_offset,(source.second + normal / norm* offset));
+ }
-/// \brief Compute spline from land way point to end point.
-/// Constraints are null velocity and acceleration.
-spline_t make_end_spline(const Point& normal, const Point& from, const Numeric offset,
- const Time init_time, const Time time_offset)
-{
- Numeric norm = normal.norm();
- assert(norm>0.);
- Point n = normal / norm;
- Point d = offset / (time_offset*time_offset*time_offset) * -n;
- Point c = -3 * d * time_offset;
- Point b = -c * time_offset;
+ /// \brief Compute spline from land way point to end point.
+ /// Constraints are null velocity and acceleration.
+ spline_t make_end_spline(const Point& normal, const Point& from, const Numeric offset,
+ const Time init_time, const Time time_offset)
+ {
+ Numeric norm = normal.norm();
+ assert(norm>0.);
+ Point n = normal / norm;
+ Point d = offset / (time_offset*time_offset*time_offset) * -n;
+ Point c = -3 * d * time_offset;
+ Point b = -c * time_offset;
+ T_Point points;
+ points.push_back(from);
+ points.push_back(b);
+ points.push_back(c);
+ points.push_back(d);
+ return spline_t(points.begin(), points.end(), init_time, init_time+time_offset);
+ }
- T_Point points;
- points.push_back(from);
- points.push_back(b);
- points.push_back(c);
- points.push_back(d);
+ /// \brief Compute end velocity : along landing normal and respecting time.
+ spline_constraints_t compute_required_offset_velocity_acceleration(const spline_t& end_spline, const Time /*time_offset*/)
+ {
+ spline_constraints_t constraints;
+ constraints.end_acc = end_spline.derivate(end_spline.min(),2);
+ constraints.end_vel = end_spline.derivate(end_spline.min(),1);
+ return constraints;
+ }
- return spline_t(points.begin(), points.end(), init_time, init_time+time_offset);
-}
-/// \brief Compute end velocity : along landing normal and respecting time.
-spline_constraints_t compute_required_offset_velocity_acceleration(const spline_t& end_spline, const Time /*time_offset*/)
-{
- spline_constraints_t constraints;
- constraints.end_acc = end_spline.derivate(end_spline.min(),2);
- constraints.end_vel = end_spline.derivate(end_spline.min(),1);
- return constraints;
-}
-
-
-/// \brief Helper method to create a spline typically used to
-/// 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.
-/// + 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 last element of a waypoint container.
-/// \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>
-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 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, 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, -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,land_offset_duration);
- spline_constraints_t constraints = compute_required_offset_velocity_acceleration(end_spline,land_offset_duration);
- exact_cubic_t all_but_end(waypoints.begin(), waypoints.end(),constraints);
- t_spline_t splines = all_but_end.curves_;
- splines.push_back(end_spline);
- return new exact_cubic_t(splines);
-}
-} // namespace helpers
+ /// \brief Helper method to create a spline typically used to
+ /// 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.
+ /// + 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 last element of a waypoint container.
+ /// \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>
+ 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 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, 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, -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,land_offset_duration);
+ spline_constraints_t constraints = compute_required_offset_velocity_acceleration(end_spline,land_offset_duration);
+ exact_cubic_t all_but_end(waypoints.begin(), waypoints.end(),constraints);
+ t_spline_t splines = all_but_end.curves_;
+ splines.push_back(end_spline);
+ return new exact_cubic_t(splines);
+ }
+ } // namespace helpers
} // namespace curves
#endif //_CLASS_EFFECTORSPLINE
diff --git a/include/curves/helpers/effector_spline_rotation.h b/include/curves/helpers/effector_spline_rotation.h
index 981b1a0..5716afb 100644
--- a/include/curves/helpers/effector_spline_rotation.h
+++ b/include/curves/helpers/effector_spline_rotation.h
@@ -26,233 +26,234 @@
namespace curves
{
-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 curve_abc<Time, Numeric, 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 exact_cubic <Numeric, Numeric, 1, false, point_one_dim_t> exact_cubic_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 rotation_spline: public curve_abc_quat_t
-{
- public:
- 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())
- {}
+ 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 curve_abc<Time, Numeric, 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 exact_cubic <Numeric, Numeric, 1, false, point_one_dim_t> exact_cubic_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;
- ~rotation_spline() {}
-
- /* Copy Constructors / operator=*/
- rotation_spline& operator=(const rotation_spline& from)
+ class rotation_spline: public curve_abc_quat_t
{
- quat_from_ = from.quat_from_;
- quat_to_ = from.quat_to_;
- min_ =from.min_; max_ = from.max_;
- time_reparam_ = exact_cubic_constraint_one_dim(from.time_reparam_);
- return *this;
- }
- /* Copy Constructors / operator=*/
+ public:
+ 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())
+ {}
- 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();
- }
+ ~rotation_spline() {}
- virtual quat_t derivate(time_t /*t*/, std::size_t /*order*/) const
- {
- throw std::runtime_error("TODO quaternion spline does not implement derivate");
- }
+ /* 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_ = exact_cubic_constraint_one_dim(from.time_reparam_);
+ return *this;
+ }
+ /* Copy Constructors / operator=*/
- /// \brief Initialize time reparametrization for spline.
- exact_cubic_constraint_one_dim computeWayPoints() const
- {
- 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 exact_cubic_constraint_one_dim(waypoints.begin(), waypoints.end());
- }
+ 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 time_t min() const{return min_;}
- virtual time_t max() const{return max_;}
+ virtual quat_t derivate(time_t /*t*/, std::size_t /*order*/) const
+ {
+ throw std::runtime_error("TODO quaternion spline does not implement derivate");
+ }
- public:
- Eigen::Quaterniond quat_from_; //const
- Eigen::Quaterniond quat_to_; //const
- double min_; //const
- double max_; //const
- exact_cubic_constraint_one_dim time_reparam_; //const
-};
+ /// \brief Initialize time reparametrization for spline.
+ exact_cubic_constraint_one_dim computeWayPoints() const
+ {
+ 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 exact_cubic_constraint_one_dim(waypoints.begin(), waypoints.end());
+ }
+ virtual time_t min() const{return min_;}
+ virtual time_t max() const{return max_;}
-typedef exact_cubic<Time, Numeric, 4, false, quat_t, std::vector<quat_t,Eigen::aligned_allocator<quat_t> >, rotation_spline> exact_cubic_quat_t;
+ /*Attributes*/
+ Eigen::Quaterniond quat_from_; //const
+ Eigen::Quaterniond quat_to_; //const
+ double min_; //const
+ double max_; //const
+ exact_cubic_constraint_one_dim time_reparam_; //const
+ /*Attributes*/
+ }; // End class rotation_spline
-/// \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 last element 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)
- , quat_spline_(simple_quat_spline())
- {
- // NOTHING
- }
+ typedef exact_cubic<Time, Numeric, 4, false, quat_t, std::vector<quat_t,Eigen::aligned_allocator<quat_t> >, rotation_spline> exact_cubic_quat_t;
- /// \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 last element 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 last element 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))
+ /// \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
{
- // NOTHING
- }
+ /* 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 last element 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),
+ quat_spline_(simple_quat_spline())
+ {
+ // NOTHING
+ }
- ~effector_spline_rotation() {delete spline_;}
- /* Constructors - destructors */
+ /// \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 last element 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 last element 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
+ }
- /*Helpers*/
- public:
- Numeric min() const{return spline_->min();}
- Numeric max() const{return spline_->max();}
- /*Helpers*/
+ ~effector_spline_rotation() {delete spline_;}
+ /* Constructors - destructors */
- /*Operations*/
- public:
- /// \brief Evaluation of the effector position and rotation at time t.
- /// \param t : the time when to evaluate the spline.
- /// \return A 7D vector where the 3 first values are the 3D position and the 4 last are 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;
- }
+ /*Helpers*/
+ Numeric min() const{return spline_->min();}
+ Numeric max() const{return spline_->max();}
+ /*Helpers*/
- 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();
- return quat_spline_(t);
- }
+ /*Operations*/
+ /// \brief Evaluation of the effector position and rotation at time t.
+ /// \param t : the time when to evaluate the spline.
+ /// \return A 7D vector where the 3 first values are the 3D position and the 4 last are 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;
+ }
- 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);
- }
+ 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();
+ return quat_spline_(t);
+ }
- template <typename InQuat>
- exact_cubic_quat_t quat_spline(InQuat quatWayPointsBegin, InQuat quatWayPointsEnd) const
- {
- if(std::distance(quatWayPointsBegin, quatWayPointsEnd) < 1)
+ private:
+ exact_cubic_quat_t simple_quat_spline() const
{
- return simple_quat_spline();
+ 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);
}
- 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)
+
+ 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);
}
- splines.push_back(rotation_spline(current_quat.coeffs(), land_quat_.coeffs(), init, time_land_offset_));
- return exact_cubic_quat_t(splines);
- }
-
- /*Operations*/
+ /*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_;
- const exact_cubic_quat_t quat_spline_;
- /*Attributes*/
-};
+ public:
+ /*Attributes*/
+ 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_;
+ const exact_cubic_quat_t quat_spline_;
+ /*Attributes*/
+ }; // End class effector_spline_rotation
-} // namespace helpers
-} // namespace curves
+ } // namespace helpers
+ } // namespace curves
#endif //_CLASS_EFFECTOR_SPLINE_ROTATION
diff --git a/include/curves/optimization/OptimizeSpline.h b/include/curves/optimization/OptimizeSpline.h
index 85fbb64..b8c4e20 100644
--- a/include/curves/optimization/OptimizeSpline.h
+++ b/include/curves/optimization/OptimizeSpline.h
@@ -22,510 +22,509 @@
namespace curves
{
-/// \class SplineOptimizer
-/// \brief Mosek connection to produce optimized splines
-template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
-, typename Point= Eigen::Matrix<Numeric, Dim, 1> >
-struct SplineOptimizer
-{
-typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
-typedef Point point_t;
-typedef Time time_t;
-typedef Numeric num_t;
-typedef exact_cubic<time_t, Numeric, Dim, Safe, Point> exact_cubic_t;
-typedef SplineOptimizer<time_t, Numeric, Dim, Safe, Point> splineOptimizer_t;
-
-/* Constructors - destructors */
-public:
- ///\brief Initializes optimizer environment.
- SplineOptimizer()
- {
- MSKrescodee r_ = MSK_makeenv(&env_,NULL);
- assert(r_ == MSK_RES_OK);
- }
-
- ///\brief Destructor.
- ~SplineOptimizer()
- {
- MSK_deleteenv(&env_);
- }
-
-private:
- SplineOptimizer(const SplineOptimizer&);
- SplineOptimizer& operator=(const SplineOptimizer&);
-/* Constructors - destructors */
-
-/*Operations*/
-public:
- /// \brief Start an optimization loop to create curve.
- /// \param waypoints : a list containing at least 2 waypoints in ascending time order.
- /// \return An Optimised curve.
- template<typename In>
- exact_cubic_t* GenerateOptimizedCurve(In wayPointsBegin, In wayPointsEnd) const;
-/*Operations*/
-
-private:
- template<typename In>
- void ComputeHMatrices(In wayPointsBegin, In wayPointsEnd,
- MatrixX& h1, MatrixX& h2, MatrixX& h3, MatrixX& h4) const;
-
-/*Attributes*/
-private:
- MSKenv_t env_;
-/*Attributes*/
-
-private:
- typedef std::pair<time_t, Point> waypoint_t;
- typedef std::vector<waypoint_t> T_waypoints_t;
-};
-
-template<typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point>
-template<typename In>
-inline void SplineOptimizer<Time, Numeric, Dim, Safe, Point>::ComputeHMatrices(In wayPointsBegin, In wayPointsEnd,
- MatrixX& h1, MatrixX& h2, MatrixX& h3, MatrixX& h4) const
-{
- std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
- assert((!Safe) || (size > 1));
-
- In it(wayPointsBegin), next(wayPointsBegin);
- ++next;
- Numeric t_previous((*it).first);
-
- for(std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i)
- {
- num_t const dTi((*next).first - (*it).first);
- num_t const dTi_sqr(dTi * dTi);
- // filling matrices values
- h3(i,i) = -3 / dTi_sqr;
- h3(i,i+1) = 3 / dTi_sqr;
- h4(i,i) = -2 / dTi;
- h4(i,i+1) = -1 / dTi;
- if( i+2 < size)
- {
- In it2(next); ++ it2;
- num_t const dTi_1((*it2).first - (*next).first);
- num_t const dTi_1sqr(dTi_1 * dTi_1);
- // this can be optimized but let's focus on clarity as long as not needed
- h1(i+1, i) = 2 / dTi;
- h1(i+1, i+1) = 4 / dTi + 4 / dTi_1;
- h1(i+1, i+2) = 2 / dTi_1;
- h2(i+1, i) = -6 / dTi_sqr;
- h2(i+1, i+1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
- h2(i+1, i+2) = 6 / dTi_1sqr;
- }
- }
-}
-
-template<typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point>
-template<typename In>
-inline exact_cubic<Time, Numeric, Dim, Safe, Point>*
- SplineOptimizer<Time, Numeric, Dim, Safe, Point>::GenerateOptimizedCurve(In wayPointsBegin, In wayPointsEnd) const
-{
- exact_cubic_t* res = 0;
- int const size((int)std::distance(wayPointsBegin, wayPointsEnd));
- if(Safe && size < 1)
- {
- throw std::length_error("can not generate optimizedCurve, number of waypoints should be superior to one");; // TODO
- }
- // refer to the paper to understand all this.
- MatrixX h1 = MatrixX::Zero(size, size);
- MatrixX h2 = MatrixX::Zero(size, size);
- MatrixX h3 = MatrixX::Zero(size, size);
- MatrixX h4 = MatrixX::Zero(size, size);
-
- // remove this for the time being
- /*MatrixX g1 = MatrixX::Zero(size, size);
- MatrixX g2 = MatrixX::Zero(size, size);
- MatrixX g3 = MatrixX::Zero(size, size);
- MatrixX g4 = MatrixX::Zero(size, size);*/
-
- ComputeHMatrices(wayPointsBegin, wayPointsEnd, h1, h2, h3, h4);
-
- // number of Waypoints : T + 1 => T mid points. Dim variables per points, + acceleration + derivations
- // (T * t+ 1 ) * Dim * 3 = nb var
-
-// NOG const MSKint32t numvar = ( size + size - 1) * 3 * 3;
- const MSKint32t numvar = (size) * Dim * 3;
- /*
- We store the variables in that order to simplifly matrix computation ( see later )
-// NOG [ x0 x1 --- xn y0 --- y z0 --- zn x0. --- zn. x0..--- zn.. x0' --- zn..' ] T
- [ x0 x1 --- xn y0 --- y z0 --- zn x0. --- zn. x0..--- zn..] T
- */
-
- /*the first constraint is H1x. = H2x => H2x - H1x. = 0
- this will give us size * 3 inequalities constraints
- So this line of A will be writen
- H2 -H1 0 0 0 0
- */
-
- int ptOff = (int) Dim * size; // . offest
- int ptptOff = (int) Dim * 2 * size; // .. offest
- int prOff = (int) 3 * ptOff; // ' offest
-// NOG int prptOff = (int) prOff + ptOff; // '. offest
-// NOG int prptptOff = (int) prptOff + ptOff; // '.. offest
-
- MatrixX h2x_h1x = MatrixX::Zero(size * Dim, numvar);
- /**A looks something like that : (n = size)
- [H2(0) 0 0 -H1(0) 0-------------------0]
- [ 0 0 H2(0) 0 0 -H1(0)---------------0]
- [ 0 0 0 H2(0) 0 0 H1(0)--------0]
- ...
- [ 0 0 0 0 H2(n) 0 0 0 -H1(n)-0] // row n
-
- Overall it's fairly easy to fill
- */
- for(int i = 0; i < size*Dim; i = i + 3)
- {
- for(int j = 0; j<Dim; ++j)
- {
- int id = i + j;
- h2x_h1x.block(id, j*size , 1, size) = h2.row(i%3);
- h2x_h1x.block(id, j*size + ptOff, 1, size) -= h1.row(i%3);
- }
- }
-
-
- /*the second constraint is x' = G1x + G2x. => G1x + G2x. - x' = 0
- this will give us size * 3 inequalities constraints
- So this line of A will be writen
- H2 -H1 0 0 0 0
- */MatrixX g1x_g2x = MatrixX::Zero(size * Dim, numvar);
- /**A looks something like that : (n = size)
- [G1(0) 0 0 G2(0) 0-----------------------0 -1 0]
- [ 0 0 G1(0) 0 0 G2(0)-------------------0 -1 0]
- [ 0 0 0 G1(0) 0 0 G2(0)-----------0 -1 0]
- ...
- [ 0 0 0 0 G1(n) 0 0 0 G2(n)-0 -1 0] // row n
-
- Overall it's fairly easy to fill
- */
-// NOG
- /*for(int j = 0; j<3; ++j)
- {
- for(int j =0; j<3; ++j)
- {
- int id = i + j;
- g1x_g2x.block(id, j*size , 1, size) = g1.row(i);
- g1x_g2x.block(id, j*size + ptOff, 1, size) = g2.row(i);
- g1x_g2x.block(id, j*size + prOff, 1, size) -= MatrixX::Ones(1, size);
- }
- }*/
-
- /*the third constraint is x.' = G3x + G4x. => G3x + G4x. - x.' = 0
- this will give us size * 3 inequalities constraints
- So this line of A will be writen
- H2 -H1 0 0 0 0
- */MatrixX g3x_g4x = MatrixX::Zero(size * Dim, numvar);
- /**A looks something like that : (n = size)
- [G3(0) 0 0 G4(0) 0-------------------0 -1 0]
- [ 0 0 G3(0) 0 0 G4(0)---------------0 -1 0]
- [ 0 0 0 G3(0) 0 0 G4(0)--------0 -1 0]
- ...
- [ 0 0 0 0 G3(n) 0 0 0 G4(n)-0 -1 0] // row n
-
- Overall it's fairly easy to fill
- */
-// NOG
- /*for(int j = 0; j<3; ++j)
- {
- for(int j =0; j<3; ++j)
- {
- int id = i + j;
- g3x_g4x.block(id, j*size , 1, size) = g3.row(i);
- g3x_g4x.block(id, j*size + ptOff, 1, size) = g4.row(i);
- g3x_g4x.block(id, j*size + prptOff, 1, size) -= MatrixX::Ones(1, size);
- }
- }
-*/
- /*the fourth constraint is x.. = 1/2(H3x + H4x.) => 1/2(H3x + H4x.) - x.. = 0
- => H3x + H4x. - 2x.. = 0
- this will give us size * 3 inequalities constraints
- So this line of A will be writen
- H2 -H1 0 0 0 0
- */
- MatrixX h3x_h4x = MatrixX::Zero(size * Dim, numvar);
- /**A looks something like that : (n = size)
- [H3(0) 0 0 H4(0) 0-------------------0 -2 0]
- [ 0 0 H3(0) 0 0 H4(0)---------------0 -2 0]
- [ 0 0 0 H3(0) 0 0 H4(0)-------0 -2 0]
- ...
- [ 0 0 0 0 H3(n) 0 0 0 H4(n)-0 -2 0] // row n
-
- Overall it's fairly easy to fill
- */
- for(int i = 0; i < size*Dim; i = i + Dim)
- {
- for(int j = 0; j<Dim; ++j)
- {
- int id = i + j;
- h3x_h4x.block(id, j*size , 1, size) = h3.row(i%3);
- h3x_h4x.block(id, j*size + ptOff , 1, size) = h4.row(i%3);
- h3x_h4x.block(id, j*size + ptptOff, 1, size) = MatrixX::Ones(1, size) * -2;
- }
- }
-
- /*the following constraints are easy to understand*/
-
- /*x0,: = x^0*/
- MatrixX x0_x0 = MatrixX::Zero(Dim, numvar);
- for(int j = 0; j<Dim; ++j)
- {
- x0_x0(0, 0) = 1;
- x0_x0(1, size) = 1;
- x0_x0(2, size * 2) = 1;
- }
-
- /*x0.,: = 0*/
- MatrixX x0p_0 = MatrixX::Zero(Dim, numvar);
- for(int j = 0; j<Dim; ++j)
- {
- x0p_0(0, ptOff) = 1;
- x0p_0(1, ptOff + size) = 1;
- x0p_0(2, ptOff + size * 2) = 1;
- }
-
- /*xt,: = x^t*/
- MatrixX xt_xt = MatrixX::Zero(Dim, numvar);
- for(int j = 0; j<Dim; ++j)
- {
- xt_xt(0, size - 1) = 1;
- xt_xt(1, 2 * size - 1) = 1;
- xt_xt(2, 3* size - 1) = 1;
- }
-
- /*xT.,: = 0*/
- MatrixX xtp_0 = MatrixX::Zero(Dim, numvar);
- for(int j = 0; j<Dim; ++j)
- {
- xtp_0(0, ptOff + size - 1) = 1;
- xtp_0(1, ptOff + size + size - 1) = 1;
- xtp_0(2, ptOff + size * 2 + size - 1)= 1;
- }
-
- //skipping constraints on x and y accelerations for the time being
- // to compute A i'll create an eigen matrix, then i'll convert it to a sparse one and fill those tables
-
- //total number of constraints
-// NOG h2x_h1x (size * Dim) + h3x_h4x (size * Dim ) + g1x_g2x (size * Dim ) + g3x_g4x (size*Dim)
-// NOG + x0_x0 (Dim ) + x0p_0 (Dim) + xt_xt (Dim) + xtp_0 (Dim) = 4 * Dim * size + 4 * Dim
- // h2x_h1x (size * Dim) + h3x_h4x (size * Dim )
- // + x0_x0 (Dim ) + x0p_0 (Dim) + xt_xt (Dim) + xtp_0 (Dim) = 2 * Dim * size + 4 * Dim
-// NOG const MSKint32t numcon = 12 * size + 12;
- const MSKint32t numcon = Dim * 2 * size + 4 * Dim; // TODO
-
- //this gives us the matrix A of size numcon * numvaar
- MatrixX a = MatrixX::Zero(numcon, numvar);
- a.block(0 , 0, size * Dim, numvar) = h2x_h1x;
- a.block(size * Dim , 0, size * Dim, numvar) = h3x_h4x;
- a.block(size * Dim * 2 , 0, Dim, numvar) = x0p_0 ;
- a.block(size * Dim * 2 + Dim , 0, Dim, numvar) = xtp_0 ;
- a.block(size * Dim * 2 + Dim * 2 , 0, Dim, numvar) = x0_x0 ;
- a.block(size * Dim * 2 + Dim * 3 , 0, Dim, numvar) = xt_xt ;
-
- //convert to sparse representation
- Eigen::SparseMatrix<Numeric> spA;
- spA = a.sparseView();
-
- //convert to sparse representation using column
- // http://docs.mosek.com/7.0/capi/Conventions_employed_in_the_API.html#sec-intro-subsubsec-cmo-rmo-matrix
-
- int nonZeros = spA.nonZeros();
-
- /* Below is the sparse representation of the A
- matrix stored by column. */
- double* aval = new double[nonZeros];
- MSKint32t* asub = new MSKint32t[nonZeros];
- MSKint32t* aptrb = new MSKint32t[numvar];
- MSKint32t* aptre = new MSKint32t[numvar];
-
- int currentIndex = 0;
- for(int j=0; j<numvar; ++j)
- {
- bool nonZeroAtThisCol = false;
- for(int i=0; i<numcon; ++i)
- {
- if(a(i,j) != 0)
- {
- if(!nonZeroAtThisCol)
- {
- aptrb[j] = currentIndex;
- nonZeroAtThisCol = true;
- }
- aval[currentIndex] = a(i,j);
- asub[currentIndex] = i;
- aptre[j] = currentIndex + 1; //overriding previous value
- ++currentIndex;
- }
- }
- }
-
- /*Q looks like this
- 0 0 0 0 0 0 | -> size * 3
- 0 0 2 0 0 0 | -> size *3
- 0 0 0 0 0 0
- 0 0 0 0 2 0
- 0 0 0 0 0 0
- */
- /* Number of non-zeros in Q.*/
- const MSKint32t numqz = size * Dim * 2;
- MSKint32t* qsubi = new MSKint32t[numqz];
- MSKint32t* qsubj = new MSKint32t[numqz];
- double* qval = new double [numqz];
- for(int id = 0; id < numqz; ++id)
- {
- qsubi[id] = id + ptOff; // we want the x.
- qsubj[id] = id + ptOff;
- qval[id] = 2;
- }
-
- /* Bounds on constraints. */
- MSKboundkeye* bkc = new MSKboundkeye[numcon];
-
- double* blc = new double[numcon];
- double* buc = new double[numcon];
-
- for(int i = 0; i < numcon - Dim * 2 ; ++i)
- {
- bkc[i] = MSK_BK_FX;
- blc[i] = 0;
- buc[i] = 0;
- }
- for(int i = numcon - Dim * 2; i < numcon - Dim ; ++i) // x0 = x^0
- {
- bkc[i] = MSK_BK_FX;
- blc[i] = wayPointsBegin->second(i - (numcon - Dim * 2) );
- buc[i] = wayPointsBegin->second(i - (numcon - Dim * 2) );
- }
- In last(wayPointsEnd);
- --last;
- for(int i = numcon - 3; i < numcon ; ++i) // xT = x^T
- {
- bkc[i] = MSK_BK_FX;
- blc[i] = last->second(i - (numcon - Dim) );
- buc[i] = last->second(i - (numcon - Dim) );
- }
-
- ///*No Bounds on variables. */
- MSKboundkeye* bkx = new MSKboundkeye[numvar];
-
- double* blx = new double[numvar];
- double* bux = new double[numvar];
-
- for(int i = 0; i < numvar; ++i)
- {
- bkx[i] = MSK_BK_FR;
- blx[i] = -MSK_INFINITY;
- bux[i] = +MSK_INFINITY;
- }
-
- MSKrescodee r;
- MSKtask_t task = NULL;
- /* Create the optimization task. */
- r = MSK_maketask(env_,numcon,numvar,&task);
-
- /* Directs the log task stream to the 'printstr' function. */
- /*if ( r==MSK_RES_OK )
- r = MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr);*/
-
- /* Append 'numcon' empty constraints.
- The constraints will initially have no bounds. */
- if ( r == MSK_RES_OK )
- {
- r = MSK_appendcons(task,numcon);
- }
-
- /* Append 'numvar' variables.
- The variables will initially be fixed at zero (x=0). */
- if ( r == MSK_RES_OK )
- {
- r = MSK_appendvars(task,numvar);
- }
-
- for(int j=0; j<numvar && r == MSK_RES_OK; ++j)
- {
- /* Set the linear term c_j in the objective.*/
- if(r == MSK_RES_OK)
- {
- r = MSK_putcj(task,j,0);
- }
- /* Set the bounds on variable j.
- blx[j] <= x_j <= bux[j] */
- if(r == MSK_RES_OK)
- {
- r = MSK_putvarbound(task,
- j, /* Index of variable.*/
- bkx[j], /* Bound key.*/
- blx[j], /* Numerical value of lower bound.*/
- bux[j]); /* Numerical value of upper bound.*/
- }
- /* Input column j of A */
- if(r == MSK_RES_OK)
- {
- r = MSK_putacol(task,
- j, /* Variable (column) index.*/
- aptre[j]-aptrb[j], /* Number of non-zeros in column j.*/
- asub+aptrb[j], /* Pointer to row indexes of column j.*/
- aval+aptrb[j]); /* Pointer to Values of column j.*/
- }
- }
-
- /* Set the bounds on constraints.
- for i=1, ...,numcon : blc[i] <= constraint i <= buc[i] */
- for(int i=0; i<numcon && r == MSK_RES_OK; ++i)
- {
- r = MSK_putconbound(task,
- i, /* Index of constraint.*/
- bkc[i], /* Bound key.*/
- blc[i], /* Numerical value of lower bound.*/
- buc[i]); /* Numerical value of upper bound.*/
- }
-
- /* Maximize objective function. */
- if (r == MSK_RES_OK)
- {
- r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MINIMIZE);
- }
-
-
- if ( r==MSK_RES_OK )
- {
- /* Input the Q for the objective. */
- r = MSK_putqobj(task,numqz,qsubi,qsubj,qval);
- }
-
- if ( r==MSK_RES_OK )
- {
- MSKrescodee trmcode;
-
- /* Run optimizer */
- r = MSK_optimizetrm(task,&trmcode);
- if ( r==MSK_RES_OK )
- {
- double *xx = (double*) calloc(numvar,sizeof(double));
- if ( xx )
- {
- /* Request the interior point solution. */
- MSK_getxx(task, MSK_SOL_ITR, xx);
- T_waypoints_t nwaypoints;
- In begin(wayPointsBegin);
- for(int i=0; i< size; i = ++i, ++begin)
- {
- point_t target;
- for(int j=0; j< Dim; ++ j)
- {
- target(j) = xx[i + j*size];
- }
- nwaypoints.push_back(std::make_pair(begin->first, target));
- }
- res = new exact_cubic_t(nwaypoints.begin(), nwaypoints.end());
- free(xx);
- }
- }
- }
- /* Delete the task and the associated data. */
- MSK_deletetask(&task);
- return res;
-}
-
+ /// \class SplineOptimizer
+ /// \brief Mosek connection to produce optimized splines
+ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
+ typename Point= Eigen::Matrix<Numeric, Dim, 1> >
+ struct SplineOptimizer
+ {
+ typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
+ typedef Point point_t;
+ typedef Time time_t;
+ typedef Numeric num_t;
+ typedef exact_cubic<time_t, Numeric, Dim, Safe, Point> exact_cubic_t;
+ typedef SplineOptimizer<time_t, Numeric, Dim, Safe, Point> splineOptimizer_t;
+
+ /* Constructors - destructors */
+ public:
+ ///\brief Initializes optimizer environment.
+ SplineOptimizer()
+ {
+ MSKrescodee r_ = MSK_makeenv(&env_,NULL);
+ assert(r_ == MSK_RES_OK);
+ }
+
+ ///\brief Destructor.
+ ~SplineOptimizer()
+ {
+ MSK_deleteenv(&env_);
+ }
+
+ private:
+ SplineOptimizer(const SplineOptimizer&);
+ SplineOptimizer& operator=(const SplineOptimizer&);
+ /* Constructors - destructors */
+
+ /*Operations*/
+ public:
+ /// \brief Start an optimization loop to create curve.
+ /// \param waypoints : a list containing at least 2 waypoints in ascending time order.
+ /// \return An Optimised curve.
+ template<typename In>
+ exact_cubic_t* GenerateOptimizedCurve(In wayPointsBegin, In wayPointsEnd) const;
+ /*Operations*/
+
+ private:
+ template<typename In>
+ void ComputeHMatrices(In wayPointsBegin, In wayPointsEnd,
+ MatrixX& h1, MatrixX& h2, MatrixX& h3, MatrixX& h4) const;
+
+ /*Attributes*/
+ MSKenv_t env_;
+ /*Attributes*/
+
+ private:
+ typedef std::pair<time_t, Point> waypoint_t;
+ typedef std::vector<waypoint_t> T_waypoints_t;
+ }; // End struct SplineOptimizer
+
+ template<typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point>
+ template<typename In>
+ inline void SplineOptimizer<Time, Numeric, Dim, Safe, Point>::ComputeHMatrices(In wayPointsBegin, In wayPointsEnd,
+ MatrixX& h1, MatrixX& h2,
+ MatrixX& h3, MatrixX& h4) const
+ {
+ std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
+ assert((!Safe) || (size > 1));
+
+ In it(wayPointsBegin), next(wayPointsBegin);
+ ++next;
+ Numeric t_previous((*it).first);
+
+ for(std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i)
+ {
+ num_t const dTi((*next).first - (*it).first);
+ num_t const dTi_sqr(dTi * dTi);
+ // filling matrices values
+ h3(i,i) = -3 / dTi_sqr;
+ h3(i,i+1) = 3 / dTi_sqr;
+ h4(i,i) = -2 / dTi;
+ h4(i,i+1) = -1 / dTi;
+ if( i+2 < size)
+ {
+ In it2(next); ++ it2;
+ num_t const dTi_1((*it2).first - (*next).first);
+ num_t const dTi_1sqr(dTi_1 * dTi_1);
+ // this can be optimized but let's focus on clarity as long as not needed
+ h1(i+1, i) = 2 / dTi;
+ h1(i+1, i+1) = 4 / dTi + 4 / dTi_1;
+ h1(i+1, i+2) = 2 / dTi_1;
+ h2(i+1, i) = -6 / dTi_sqr;
+ h2(i+1, i+1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
+ h2(i+1, i+2) = 6 / dTi_1sqr;
+ }
+ }
+ }
+
+ template<typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point>
+ template<typename In>
+ inline exact_cubic<Time, Numeric, Dim, Safe, Point>*
+ SplineOptimizer<Time, Numeric, Dim, Safe, Point>::GenerateOptimizedCurve(In wayPointsBegin, In wayPointsEnd) const
+ {
+ exact_cubic_t* res = 0;
+ int const size((int)std::distance(wayPointsBegin, wayPointsEnd));
+ if(Safe && size < 1)
+ {
+ throw std::length_error("can not generate optimizedCurve, number of waypoints should be superior to one");; // TODO
+ }
+ // refer to the paper to understand all this.
+ MatrixX h1 = MatrixX::Zero(size, size);
+ MatrixX h2 = MatrixX::Zero(size, size);
+ MatrixX h3 = MatrixX::Zero(size, size);
+ MatrixX h4 = MatrixX::Zero(size, size);
+
+ // remove this for the time being
+ /*MatrixX g1 = MatrixX::Zero(size, size);
+ MatrixX g2 = MatrixX::Zero(size, size);
+ MatrixX g3 = MatrixX::Zero(size, size);
+ MatrixX g4 = MatrixX::Zero(size, size);*/
+
+ ComputeHMatrices(wayPointsBegin, wayPointsEnd, h1, h2, h3, h4);
+
+ // number of Waypoints : T + 1 => T mid points. Dim variables per points, + acceleration + derivations
+ // (T * t+ 1 ) * Dim * 3 = nb var
+
+ // NOG const MSKint32t numvar = ( size + size - 1) * 3 * 3;
+ const MSKint32t numvar = (size) * Dim * 3;
+ /*
+ We store the variables in that order to simplifly matrix computation ( see later )
+ // NOG [ x0 x1 --- xn y0 --- y z0 --- zn x0. --- zn. x0..--- zn.. x0' --- zn..' ] T
+ [ x0 x1 --- xn y0 --- y z0 --- zn x0. --- zn. x0..--- zn..] T
+ */
+
+ /*the first constraint is H1x. = H2x => H2x - H1x. = 0
+ this will give us size * 3 inequalities constraints
+ So this line of A will be writen
+ H2 -H1 0 0 0 0
+ */
+
+ int ptOff = (int) Dim * size; // . offest
+ int ptptOff = (int) Dim * 2 * size; // .. offest
+ int prOff = (int) 3 * ptOff; // ' offest
+ // NOG int prptOff = (int) prOff + ptOff; // '. offest
+ // NOG int prptptOff = (int) prptOff + ptOff; // '.. offest
+
+ MatrixX h2x_h1x = MatrixX::Zero(size * Dim, numvar);
+ /**A looks something like that : (n = size)
+ [H2(0) 0 0 -H1(0) 0-------------------0]
+ [ 0 0 H2(0) 0 0 -H1(0)---------------0]
+ [ 0 0 0 H2(0) 0 0 H1(0)--------0]
+ ...
+ [ 0 0 0 0 H2(n) 0 0 0 -H1(n)-0] // row n
+
+ Overall it's fairly easy to fill
+ */
+ for(int i = 0; i < size*Dim; i = i + 3)
+ {
+ for(int j = 0; j<Dim; ++j)
+ {
+ int id = i + j;
+ h2x_h1x.block(id, j*size , 1, size) = h2.row(i%3);
+ h2x_h1x.block(id, j*size + ptOff, 1, size) -= h1.row(i%3);
+ }
+ }
+
+
+ /*the second constraint is x' = G1x + G2x. => G1x + G2x. - x' = 0
+ this will give us size * 3 inequalities constraints
+ So this line of A will be writen
+ H2 -H1 0 0 0 0
+ */MatrixX g1x_g2x = MatrixX::Zero(size * Dim, numvar);
+ /**A looks something like that : (n = size)
+ [G1(0) 0 0 G2(0) 0-----------------------0 -1 0]
+ [ 0 0 G1(0) 0 0 G2(0)-------------------0 -1 0]
+ [ 0 0 0 G1(0) 0 0 G2(0)-----------0 -1 0]
+ ...
+ [ 0 0 0 0 G1(n) 0 0 0 G2(n)-0 -1 0] // row n
+
+ Overall it's fairly easy to fill
+ */
+ // NOG
+ /*for(int j = 0; j<3; ++j)
+ {
+ for(int j =0; j<3; ++j)
+ {
+ int id = i + j;
+ g1x_g2x.block(id, j*size , 1, size) = g1.row(i);
+ g1x_g2x.block(id, j*size + ptOff, 1, size) = g2.row(i);
+ g1x_g2x.block(id, j*size + prOff, 1, size) -= MatrixX::Ones(1, size);
+ }
+ }*/
+
+ /*the third constraint is x.' = G3x + G4x. => G3x + G4x. - x.' = 0
+ this will give us size * 3 inequalities constraints
+ So this line of A will be writen
+ H2 -H1 0 0 0 0
+ */MatrixX g3x_g4x = MatrixX::Zero(size * Dim, numvar);
+ /**A looks something like that : (n = size)
+ [G3(0) 0 0 G4(0) 0-------------------0 -1 0]
+ [ 0 0 G3(0) 0 0 G4(0)---------------0 -1 0]
+ [ 0 0 0 G3(0) 0 0 G4(0)--------0 -1 0]
+ ...
+ [ 0 0 0 0 G3(n) 0 0 0 G4(n)-0 -1 0] // row n
+
+ Overall it's fairly easy to fill
+ */
+ // NOG
+ /*for(int j = 0; j<3; ++j)
+ {
+ for(int j =0; j<3; ++j)
+ {
+ int id = i + j;
+ g3x_g4x.block(id, j*size , 1, size) = g3.row(i);
+ g3x_g4x.block(id, j*size + ptOff, 1, size) = g4.row(i);
+ g3x_g4x.block(id, j*size + prptOff, 1, size) -= MatrixX::Ones(1, size);
+ }
+ }
+ */
+ /*the fourth constraint is x.. = 1/2(H3x + H4x.) => 1/2(H3x + H4x.) - x.. = 0
+ => H3x + H4x. - 2x.. = 0
+ this will give us size * 3 inequalities constraints
+ So this line of A will be writen
+ H2 -H1 0 0 0 0
+ */
+ MatrixX h3x_h4x = MatrixX::Zero(size * Dim, numvar);
+ /**A looks something like that : (n = size)
+ [H3(0) 0 0 H4(0) 0-------------------0 -2 0]
+ [ 0 0 H3(0) 0 0 H4(0)---------------0 -2 0]
+ [ 0 0 0 H3(0) 0 0 H4(0)-------0 -2 0]
+ ...
+ [ 0 0 0 0 H3(n) 0 0 0 H4(n)-0 -2 0] // row n
+
+ Overall it's fairly easy to fill
+ */
+ for(int i = 0; i < size*Dim; i = i + Dim)
+ {
+ for(int j = 0; j<Dim; ++j)
+ {
+ int id = i + j;
+ h3x_h4x.block(id, j*size , 1, size) = h3.row(i%3);
+ h3x_h4x.block(id, j*size + ptOff , 1, size) = h4.row(i%3);
+ h3x_h4x.block(id, j*size + ptptOff, 1, size) = MatrixX::Ones(1, size) * -2;
+ }
+ }
+
+ /*the following constraints are easy to understand*/
+
+ /*x0,: = x^0*/
+ MatrixX x0_x0 = MatrixX::Zero(Dim, numvar);
+ for(int j = 0; j<Dim; ++j)
+ {
+ x0_x0(0, 0) = 1;
+ x0_x0(1, size) = 1;
+ x0_x0(2, size * 2) = 1;
+ }
+
+ /*x0.,: = 0*/
+ MatrixX x0p_0 = MatrixX::Zero(Dim, numvar);
+ for(int j = 0; j<Dim; ++j)
+ {
+ x0p_0(0, ptOff) = 1;
+ x0p_0(1, ptOff + size) = 1;
+ x0p_0(2, ptOff + size * 2) = 1;
+ }
+
+ /*xt,: = x^t*/
+ MatrixX xt_xt = MatrixX::Zero(Dim, numvar);
+ for(int j = 0; j<Dim; ++j)
+ {
+ xt_xt(0, size - 1) = 1;
+ xt_xt(1, 2 * size - 1) = 1;
+ xt_xt(2, 3* size - 1) = 1;
+ }
+
+ /*xT.,: = 0*/
+ MatrixX xtp_0 = MatrixX::Zero(Dim, numvar);
+ for(int j = 0; j<Dim; ++j)
+ {
+ xtp_0(0, ptOff + size - 1) = 1;
+ xtp_0(1, ptOff + size + size - 1) = 1;
+ xtp_0(2, ptOff + size * 2 + size - 1)= 1;
+ }
+
+ //skipping constraints on x and y accelerations for the time being
+ // to compute A i'll create an eigen matrix, then i'll convert it to a sparse one and fill those tables
+
+ //total number of constraints
+ // NOG h2x_h1x (size * Dim) + h3x_h4x (size * Dim ) + g1x_g2x (size * Dim ) + g3x_g4x (size*Dim)
+ // NOG + x0_x0 (Dim ) + x0p_0 (Dim) + xt_xt (Dim) + xtp_0 (Dim) = 4 * Dim * size + 4 * Dim
+ // h2x_h1x (size * Dim) + h3x_h4x (size * Dim )
+ // + x0_x0 (Dim ) + x0p_0 (Dim) + xt_xt (Dim) + xtp_0 (Dim) = 2 * Dim * size + 4 * Dim
+ // NOG const MSKint32t numcon = 12 * size + 12;
+ const MSKint32t numcon = Dim * 2 * size + 4 * Dim; // TODO
+
+ //this gives us the matrix A of size numcon * numvaar
+ MatrixX a = MatrixX::Zero(numcon, numvar);
+ a.block(0 , 0, size * Dim, numvar) = h2x_h1x;
+ a.block(size * Dim , 0, size * Dim, numvar) = h3x_h4x;
+ a.block(size * Dim * 2 , 0, Dim, numvar) = x0p_0 ;
+ a.block(size * Dim * 2 + Dim , 0, Dim, numvar) = xtp_0 ;
+ a.block(size * Dim * 2 + Dim * 2 , 0, Dim, numvar) = x0_x0 ;
+ a.block(size * Dim * 2 + Dim * 3 , 0, Dim, numvar) = xt_xt ;
+
+ //convert to sparse representation
+ Eigen::SparseMatrix<Numeric> spA;
+ spA = a.sparseView();
+
+ //convert to sparse representation using column
+ // http://docs.mosek.com/7.0/capi/Conventions_employed_in_the_API.html#sec-intro-subsubsec-cmo-rmo-matrix
+
+ int nonZeros = spA.nonZeros();
+
+ /* Below is the sparse representation of the A
+ matrix stored by column. */
+ double* aval = new double[nonZeros];
+ MSKint32t* asub = new MSKint32t[nonZeros];
+ MSKint32t* aptrb = new MSKint32t[numvar];
+ MSKint32t* aptre = new MSKint32t[numvar];
+
+ int currentIndex = 0;
+ for(int j=0; j<numvar; ++j)
+ {
+ bool nonZeroAtThisCol = false;
+ for(int i=0; i<numcon; ++i)
+ {
+ if(a(i,j) != 0)
+ {
+ if(!nonZeroAtThisCol)
+ {
+ aptrb[j] = currentIndex;
+ nonZeroAtThisCol = true;
+ }
+ aval[currentIndex] = a(i,j);
+ asub[currentIndex] = i;
+ aptre[j] = currentIndex + 1; //overriding previous value
+ ++currentIndex;
+ }
+ }
+ }
+
+ /*Q looks like this
+ 0 0 0 0 0 0 | -> size * 3
+ 0 0 2 0 0 0 | -> size *3
+ 0 0 0 0 0 0
+ 0 0 0 0 2 0
+ 0 0 0 0 0 0
+ */
+ /* Number of non-zeros in Q.*/
+ const MSKint32t numqz = size * Dim * 2;
+ MSKint32t* qsubi = new MSKint32t[numqz];
+ MSKint32t* qsubj = new MSKint32t[numqz];
+ double* qval = new double [numqz];
+ for(int id = 0; id < numqz; ++id)
+ {
+ qsubi[id] = id + ptOff; // we want the x.
+ qsubj[id] = id + ptOff;
+ qval[id] = 2;
+ }
+
+ /* Bounds on constraints. */
+ MSKboundkeye* bkc = new MSKboundkeye[numcon];
+
+ double* blc = new double[numcon];
+ double* buc = new double[numcon];
+
+ for(int i = 0; i < numcon - Dim * 2 ; ++i)
+ {
+ bkc[i] = MSK_BK_FX;
+ blc[i] = 0;
+ buc[i] = 0;
+ }
+ for(int i = numcon - Dim * 2; i < numcon - Dim ; ++i) // x0 = x^0
+ {
+ bkc[i] = MSK_BK_FX;
+ blc[i] = wayPointsBegin->second(i - (numcon - Dim * 2) );
+ buc[i] = wayPointsBegin->second(i - (numcon - Dim * 2) );
+ }
+ In last(wayPointsEnd);
+ --last;
+ for(int i = numcon - 3; i < numcon ; ++i) // xT = x^T
+ {
+ bkc[i] = MSK_BK_FX;
+ blc[i] = last->second(i - (numcon - Dim) );
+ buc[i] = last->second(i - (numcon - Dim) );
+ }
+
+ ///*No Bounds on variables. */
+ MSKboundkeye* bkx = new MSKboundkeye[numvar];
+
+ double* blx = new double[numvar];
+ double* bux = new double[numvar];
+
+ for(int i = 0; i < numvar; ++i)
+ {
+ bkx[i] = MSK_BK_FR;
+ blx[i] = -MSK_INFINITY;
+ bux[i] = +MSK_INFINITY;
+ }
+
+ MSKrescodee r;
+ MSKtask_t task = NULL;
+ /* Create the optimization task. */
+ r = MSK_maketask(env_,numcon,numvar,&task);
+
+ /* Directs the log task stream to the 'printstr' function. */
+ /*if ( r==MSK_RES_OK )
+ r = MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr);*/
+
+ /* Append 'numcon' empty constraints.
+ The constraints will initially have no bounds. */
+ if ( r == MSK_RES_OK )
+ {
+ r = MSK_appendcons(task,numcon);
+ }
+
+ /* Append 'numvar' variables.
+ The variables will initially be fixed at zero (x=0). */
+ if ( r == MSK_RES_OK )
+ {
+ r = MSK_appendvars(task,numvar);
+ }
+
+ for(int j=0; j<numvar && r == MSK_RES_OK; ++j)
+ {
+ /* Set the linear term c_j in the objective.*/
+ if(r == MSK_RES_OK)
+ {
+ r = MSK_putcj(task,j,0);
+ }
+ /* Set the bounds on variable j.
+ blx[j] <= x_j <= bux[j] */
+ if(r == MSK_RES_OK)
+ {
+ r = MSK_putvarbound(task,
+ j, /* Index of variable.*/
+ bkx[j], /* Bound key.*/
+ blx[j], /* Numerical value of lower bound.*/
+ bux[j]); /* Numerical value of upper bound.*/
+ }
+ /* Input column j of A */
+ if(r == MSK_RES_OK)
+ {
+ r = MSK_putacol(task,
+ j, /* Variable (column) index.*/
+ aptre[j]-aptrb[j], /* Number of non-zeros in column j.*/
+ asub+aptrb[j], /* Pointer to row indexes of column j.*/
+ aval+aptrb[j]); /* Pointer to Values of column j.*/
+ }
+ }
+
+ /* Set the bounds on constraints.
+ for i=1, ...,numcon : blc[i] <= constraint i <= buc[i] */
+ for(int i=0; i<numcon && r == MSK_RES_OK; ++i)
+ {
+ r = MSK_putconbound(task,
+ i, /* Index of constraint.*/
+ bkc[i], /* Bound key.*/
+ blc[i], /* Numerical value of lower bound.*/
+ buc[i]); /* Numerical value of upper bound.*/
+ }
+
+ /* Maximize objective function. */
+ if (r == MSK_RES_OK)
+ {
+ r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MINIMIZE);
+ }
+
+
+ if ( r==MSK_RES_OK )
+ {
+ /* Input the Q for the objective. */
+ r = MSK_putqobj(task,numqz,qsubi,qsubj,qval);
+ }
+
+ if ( r==MSK_RES_OK )
+ {
+ MSKrescodee trmcode;
+
+ /* Run optimizer */
+ r = MSK_optimizetrm(task,&trmcode);
+ if ( r==MSK_RES_OK )
+ {
+ double *xx = (double*) calloc(numvar,sizeof(double));
+ if ( xx )
+ {
+ /* Request the interior point solution. */
+ MSK_getxx(task, MSK_SOL_ITR, xx);
+ T_waypoints_t nwaypoints;
+ In begin(wayPointsBegin);
+ for(int i=0; i< size; i = ++i, ++begin)
+ {
+ point_t target;
+ for(int j=0; j< Dim; ++ j)
+ {
+ target(j) = xx[i + j*size];
+ }
+ nwaypoints.push_back(std::make_pair(begin->first, target));
+ }
+ res = new exact_cubic_t(nwaypoints.begin(), nwaypoints.end());
+ free(xx);
+ }
+ }
+ }
+ /* Delete the task and the associated data. */
+ MSK_deletetask(&task);
+ return res;
+ }
} // namespace curves
#endif //_CLASS_SPLINEOPTIMIZER
diff --git a/include/curves/serialization/CMakeLists.txt b/include/curves/serialization/CMakeLists.txt
index 63009f4..5961ad6 100644
--- a/include/curves/serialization/CMakeLists.txt
+++ b/include/curves/serialization/CMakeLists.txt
@@ -1,5 +1,4 @@
SET(${PROJECT_NAME}_SERIALIZATION_HEADERS
- aligned-vector.hpp
archive.hpp
eigen-matrix.hpp
fwd.hpp
--
GitLab