Commit c55a3f93 authored by JasonChmn's avatar JasonChmn Committed by Pierre Fernbach
Browse files

Remove aligned_vector file / redo indentation in helpers

parent 882a795b
......@@ -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
SET(${PROJECT_NAME}_SERIALIZATION_HEADERS
aligned-vector.hpp
archive.hpp
eigen-matrix.hpp
fwd.hpp
......
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