19 #ifndef _CLASS_EFFECTOR_SPLINE_ROTATION
20 #define _CLASS_EFFECTOR_SPLINE_ROTATION
22 #include <Eigen/Geometry>
29 typedef Eigen::Matrix<Numeric, 4, 1>
quat_t;
46 const double min = 0.,
const double max = 1.)
89 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
90 return ndcurves::isApprox<Numeric>(
min_, other.
min_) &&
91 ndcurves::isApprox<Numeric>(
max_, other.
max_) &&
99 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
113 return !(*
this == other);
117 throw std::runtime_error(
118 "TODO quaternion spline does not implement derivate");
126 throw std::logic_error(
127 "Compute derivate for quaternion spline is not implemented yet.");
133 waypoints.push_back(std::make_pair(0, point_one_dim_t::Zero()));
134 waypoints.push_back(std::make_pair(1, point_one_dim_t::Ones()));
140 virtual std::size_t
dim()
const {
return dim_; }
149 virtual std::size_t
degree()
const {
return 1; }
162 std::vector<quat_t, Eigen::aligned_allocator<quat_t> >,
194 template <
typename In>
198 const Point& lift_normal = Eigen::Vector3d::UnitZ(),
199 const Point& land_normal = Eigen::Vector3d::UnitZ(),
200 const Numeric lift_offset = 0.02,
201 const Numeric land_offset = 0.02,
202 const Time lift_offset_duration = 0.02,
203 const Time land_offset_duration = 0.02)
205 land_normal, lift_offset, land_offset,
206 lift_offset_duration, land_offset_duration)),
237 template <
typename In,
typename InQuat>
239 InQuat quatWayPointsBegin, InQuat quatWayPointsEnd,
240 const Point& lift_normal = Eigen::Vector3d::UnitZ(),
241 const Point& land_normal = Eigen::Vector3d::UnitZ(),
242 const Numeric lift_offset = 0.02,
243 const Numeric land_offset = 0.02,
244 const Time lift_offset_duration = 0.02,
245 const Time land_offset_duration = 0.02)
247 land_normal, lift_offset, land_offset,
248 lift_offset_duration, land_offset_duration)),
249 to_quat_((quatWayPointsBegin->second).data()),
250 land_quat_(((quatWayPointsEnd - 1)->second).data()),
253 quat_spline_(quat_spline(quatWayPointsBegin, quatWayPointsEnd)) {
286 std::vector<rotation_spline> splines;
292 template <
typename InQuat>
294 InQuat quatWayPointsEnd)
const {
295 if (std::distance(quatWayPointsBegin, quatWayPointsEnd) < 1) {
296 return simple_quat_spline();
299 InQuat it(quatWayPointsBegin);
301 Eigen::Quaterniond current_quat =
to_quat_;
302 for (; it != quatWayPointsEnd; ++it) {
304 rotation_spline(current_quat.coeffs(), it->second, init, it->first));
305 current_quat = it->second;
308 splines.push_back(rotation_spline(
Represents a trajectory for and end effector. uses the method effector_spline to create a spline traj...
Definition: effector_spline_rotation.h:173
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)
Constructor. Given a set of waypoints, and the normal vector of the start and ending positions,...
Definition: effector_spline_rotation.h:238
const Eigen::Quaterniond to_quat_
Definition: effector_spline_rotation.h:317
Numeric min() const
Definition: effector_spline_rotation.h:261
const double time_land_offset_
Definition: effector_spline_rotation.h:320
const Eigen::Quaterniond land_quat_
Definition: effector_spline_rotation.h:318
quat_t interpolate_quat(const Numeric t) const
Definition: effector_spline_rotation.h:278
config_t operator()(const Numeric t) const
Evaluation of the effector position and rotation at time t.
Definition: effector_spline_rotation.h:271
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)
Constructor. Given a set of waypoints, and the normal vector of the start and ending positions,...
Definition: effector_spline_rotation.h:195
virtual ~effector_spline_rotation()
Definition: effector_spline_rotation.h:257
Numeric max() const
Definition: effector_spline_rotation.h:262
const exact_cubic_t * spline_
Definition: effector_spline_rotation.h:316
const double time_lift_offset_
Definition: effector_spline_rotation.h:319
const exact_cubic_quat_t quat_spline_
Definition: effector_spline_rotation.h:321
Definition: effector_spline_rotation.h:42
rotation_spline & operator=(const rotation_spline &from)
Definition: effector_spline_rotation.h:58
virtual time_t min() const
Get the minimum time for which the curve is defined.
Definition: effector_spline_rotation.h:143
bool isApprox(const rotation_spline &other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equals. Only two curves of the same class can be ...
Definition: effector_spline_rotation.h:87
virtual bool operator==(const rotation_spline &other) const
Definition: effector_spline_rotation.h:108
Eigen::Quaterniond quat_to_
Definition: effector_spline_rotation.h:153
virtual bool operator!=(const rotation_spline &other) const
Definition: effector_spline_rotation.h:112
exact_cubic_constraint_one_dim computeWayPoints() const
Initialize time re-parametrization for spline.
Definition: effector_spline_rotation.h:131
virtual std::size_t degree() const
Get the degree of the curve.
Definition: effector_spline_rotation.h:149
double max_
Definition: effector_spline_rotation.h:156
Eigen::Quaterniond quat_from_
Definition: effector_spline_rotation.h:152
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.)
Definition: effector_spline_rotation.h:44
curve_abc_quat_t * compute_derivate_ptr(const std::size_t) const
Compute the derived curve at order N.
Definition: effector_spline_rotation.h:125
exact_cubic_constraint_one_dim time_reparam_
Definition: effector_spline_rotation.h:157
double min_
Definition: effector_spline_rotation.h:155
virtual std::size_t dim() const
Get dimension of curve.
Definition: effector_spline_rotation.h:140
virtual time_t max() const
Get the maximum time for which the curve is defined.
Definition: effector_spline_rotation.h:146
std::size_t dim_
Definition: effector_spline_rotation.h:154
quat_t operator()(const Numeric t) const
Definition: effector_spline_rotation.h:69
virtual quat_t derivate(time_t, std::size_t) const
Definition: effector_spline_rotation.h:116
virtual bool isApprox(const curve_abc_quat_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: effector_spline_rotation.h:97
~rotation_spline()
Definition: effector_spline_rotation.h:55
interface for a Curve of arbitrary dimension.
curve_abc< Time, Numeric, false, quat_t > curve_abc_quat_t
Definition: effector_spline_rotation.h:33
exact_cubic< Numeric, Numeric, false, point_one_dim_t > exact_cubic_constraint_one_dim
Definition: effector_spline_rotation.h:38
const Eigen::Ref< const quat_t > quat_ref_const_t
Definition: effector_spline_rotation.h:31
Eigen::Matrix< Numeric, 7, 1 > config_t
Definition: effector_spline_rotation.h:32
exact_cubic< Time, Numeric, false, quat_t, std::vector< quat_t, Eigen::aligned_allocator< quat_t > >, rotation_spline > exact_cubic_quat_t
Definition: effector_spline_rotation.h:164
Eigen::Ref< quat_t > quat_ref_t
Definition: effector_spline_rotation.h:30
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
std::pair< Numeric, point_one_dim_t > waypoint_one_dim_t
Definition: effector_spline_rotation.h:39
double Numeric
Definition: effector_spline.h:26
std::vector< waypoint_one_dim_t > t_waypoint_one_dim_t
Definition: effector_spline_rotation.h:40
double Time
Definition: effector_spline.h:27
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)
Helper method to create a spline typically used to guide the 3d trajectory of a robot end effector....
Definition: effector_spline.h:97
std::vector< waypoint_quat_t > t_waypoint_quat_t
Definition: effector_spline_rotation.h:35
Eigen::Matrix< Numeric, 1, 1 > point_one_dim_t
Definition: effector_spline_rotation.h:36
Eigen::Matrix< Numeric, 4, 1 > quat_t
Definition: effector_spline_rotation.h:29
std::pair< Numeric, quat_t > waypoint_quat_t
Definition: effector_spline_rotation.h:34
Definition: bernstein.h:20
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:37
Time time_t
Definition: curve_abc.h:40
std::vector< spline_t > t_spline_t
Definition: exact_cubic.h:50
virtual Time min() const
Get the minimum time for which the curve is defined.
Definition: piecewise_curve.h:644
virtual Time max() const
Get the maximum time for which the curve is defined.
Definition: piecewise_curve.h:647
bool isApprox(const piecewise_curve_t &other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equals. Only two curves of the same class can be ...
Definition: piecewise_curve.h:112