19 #ifndef _CLASS_EFFECTOR_SPLINE_ROTATION 20 #define _CLASS_EFFECTOR_SPLINE_ROTATION 24 #include <Eigen/Geometry> 28 typedef Eigen::Matrix<Numeric, 4, 1>
quat_t;
43 const double min = 0.,
const double max = 1.)
84 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
85 return curves::isApprox<Numeric>(
min_, other.
min_) && curves::isApprox<Numeric>(
max_, other.
max_) &&
90 virtual bool isApprox(
const curve_abc_quat_t* other,
91 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
104 throw std::runtime_error(
"TODO quaternion spline does not implement derivate");
111 throw std::logic_error(
"Compute derivate for quaternion spline is not implemented yet.");
116 t_waypoint_one_dim_t waypoints;
117 waypoints.push_back(std::make_pair(0, point_one_dim_t::Zero()));
118 waypoints.push_back(std::make_pair(1, point_one_dim_t::Ones()));
124 virtual std::size_t
dim()
const {
return dim_; }
133 virtual std::size_t
degree()
const {
return 1; }
175 template <
typename In>
177 quat_ref_const_t& land_quat =
quat_t(0, 0, 0, 1),
178 const Point& lift_normal = Eigen::Vector3d::UnitZ(),
179 const Point& land_normal = Eigen::Vector3d::UnitZ(),
const Numeric lift_offset = 0.02,
180 const Numeric land_offset = 0.02,
const Time lift_offset_duration = 0.02,
181 const Time land_offset_duration = 0.02)
182 : spline_(
effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, land_normal, lift_offset, land_offset,
183 lift_offset_duration, land_offset_duration)),
184 to_quat_(to_quat.data()),
185 land_quat_(land_quat.data()),
186 time_lift_offset_(spline_->
min() + lift_offset_duration),
187 time_land_offset_(spline_->
max() - land_offset_duration),
188 quat_spline_(simple_quat_spline()) {
210 template <
typename In,
typename InQuat>
212 const Point& lift_normal = Eigen::Vector3d::UnitZ(),
213 const Point& land_normal = Eigen::Vector3d::UnitZ(),
const Numeric lift_offset = 0.02,
214 const Numeric land_offset = 0.02,
const Time lift_offset_duration = 0.02,
215 const Time land_offset_duration = 0.02)
216 : spline_(
effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, land_normal, lift_offset, land_offset,
217 lift_offset_duration, land_offset_duration)),
218 to_quat_((quatWayPointsBegin->second).data()),
219 land_quat_(((quatWayPointsEnd - 1)->second).data()),
220 time_lift_offset_(spline_->
min() + lift_offset_duration),
221 time_land_offset_(spline_->
max() - land_offset_duration),
222 quat_spline_(quat_spline(quatWayPointsBegin, quatWayPointsEnd)) {
242 res.head<3>() = (*spline_)(t);
243 res.tail<4>() = interpolate_quat(t);
248 if (t <= time_lift_offset_)
return to_quat_.coeffs();
249 if (t >= time_land_offset_)
return land_quat_.coeffs();
250 return quat_spline_(t);
255 std::vector<rotation_spline> splines;
256 splines.push_back(
rotation_spline(to_quat_.coeffs(), land_quat_.coeffs(), time_lift_offset_, time_land_offset_));
260 template <
typename InQuat>
261 exact_cubic_quat_t quat_spline(InQuat quatWayPointsBegin, InQuat quatWayPointsEnd)
const {
262 if (std::distance(quatWayPointsBegin, quatWayPointsEnd) < 1) {
263 return simple_quat_spline();
266 InQuat it(quatWayPointsBegin);
267 Time init = time_lift_offset_;
268 Eigen::Quaterniond current_quat = to_quat_;
269 for (; it != quatWayPointsEnd; ++it) {
270 splines.push_back(
rotation_spline(current_quat.coeffs(), it->second, init, it->first));
271 current_quat = it->second;
274 splines.push_back(
rotation_spline(current_quat.coeffs(), land_quat_.coeffs(), init, time_land_offset_));
292 #endif //_CLASS_EFFECTOR_SPLINE_ROTATION std::pair< Numeric, point_one_dim_t > waypoint_one_dim_t
Definition: effector_spline_rotation.h:37
Eigen::Matrix< Numeric, 7, 1 > config_t
Definition: effector_spline_rotation.h:31
curve_abc< Time, Numeric, false, quat_t > curve_abc_quat_t
Definition: effector_spline_rotation.h:32
double min_
Definition: effector_spline_rotation.h:139
const exact_cubic_quat_t quat_spline_
Definition: effector_spline_rotation.h:286
virtual bool operator!=(const rotation_spline &other) const
Definition: effector_spline_rotation.h:101
double Numeric
Definition: effector_spline.h:26
const exact_cubic_t * spline_
Definition: effector_spline_rotation.h:281
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:211
exact_cubic_constraint_one_dim time_reparam_
Definition: effector_spline_rotation.h:141
std::pair< Numeric, quat_t > waypoint_quat_t
Definition: effector_spline_rotation.h:33
interface for a Curve of arbitrary dimension.
virtual time_t max() const
Get the maximum time for which the curve is defined.
Definition: effector_spline_rotation.h:130
Eigen::Ref< quat_t > quat_ref_t
Definition: effector_spline_rotation.h:29
virtual std::size_t dim() const
Get dimension of curve.
Definition: effector_spline_rotation.h:124
Definition: bernstein.h:20
config_t operator()(const Numeric t) const
Evaluation of the effector position and rotation at time t.
Definition: effector_spline_rotation.h:240
Time time_t
Definition: curve_abc.h:37
Numeric max() const
Definition: effector_spline_rotation.h:231
const double time_land_offset_
Definition: effector_spline_rotation.h:285
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:91
Represents a trajectory for and end effector. uses the method effector_spline to create a spline traj...
Definition: effector_spline_rotation.h:156
virtual std::size_t degree() const
Get the degree of the curve.
Definition: effector_spline_rotation.h:133
Numeric min() const
Definition: effector_spline_rotation.h:230
const Eigen::Quaterniond to_quat_
Definition: effector_spline_rotation.h:282
quat_t operator()(const Numeric t) const
Definition: effector_spline_rotation.h:66
Eigen::Matrix< Numeric, 4, 1 > quat_t
Definition: effector_spline_rotation.h:28
std::vector< waypoint_quat_t > t_waypoint_quat_t
Definition: effector_spline_rotation.h:34
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:176
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:83
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:147
std::vector< spline_t > t_spline_t
Definition: exact_cubic.h:50
std::vector< waypoint_one_dim_t > t_waypoint_one_dim_t
Definition: effector_spline_rotation.h:38
quat_t interpolate_quat(const Numeric t) const
Definition: effector_spline_rotation.h:247
virtual bool operator==(const rotation_spline &other) const
Definition: effector_spline_rotation.h:99
virtual quat_t derivate(time_t, std::size_t) const
Definition: effector_spline_rotation.h:103
virtual time_t min() const
Get the minimum time for which the curve is defined.
Definition: effector_spline_rotation.h:127
~effector_spline_rotation()
Definition: effector_spline_rotation.h:226
virtual bool isApprox(const curve_abc_quat_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: effector_spline_rotation.h:90
exact_cubic< Numeric, Numeric, false, point_one_dim_t > exact_cubic_constraint_one_dim
Definition: effector_spline_rotation.h:36
const double time_lift_offset_
Definition: effector_spline_rotation.h:284
bool isApprox(const exact_cubic_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: exact_cubic.h:116
Definition: effector_spline_rotation.h:40
Eigen::Quaterniond quat_from_
Definition: effector_spline_rotation.h:136
Eigen::Quaterniond quat_to_
Definition: effector_spline_rotation.h:137
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:42
rotation_spline & operator=(const rotation_spline &from)
Definition: effector_spline_rotation.h:55
double max_
Definition: effector_spline_rotation.h:140
~rotation_spline()
Definition: effector_spline_rotation.h:52
double Time
Definition: effector_spline.h:27
curve_abc_quat_t * compute_derivate_ptr(const std::size_t) const
Compute the derived curve at order N.
Definition: effector_spline_rotation.h:110
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
std::size_t dim_
Definition: effector_spline_rotation.h:138
const Eigen::Ref< const quat_t > quat_ref_const_t
Definition: effector_spline_rotation.h:30
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:34
exact_cubic_constraint_one_dim computeWayPoints() const
Initialize time reparametrization for spline.
Definition: effector_spline_rotation.h:115
const Eigen::Quaterniond land_quat_
Definition: effector_spline_rotation.h:283
Eigen::Matrix< Numeric, 1, 1 > point_one_dim_t
Definition: effector_spline_rotation.h:35