Go to the documentation of this file.
19 #ifndef _CLASS_EFFECTOR_SPLINE_ROTATION
20 #define _CLASS_EFFECTOR_SPLINE_ROTATION
22 #include "spline/helpers/effector_spline.h"
23 #include "spline/curve_abc.h"
24 #include <Eigen/Geometry>
29 typedef Eigen::Matrix<Numeric, 4, 1>
quat_t;
44 const double min = 0.,
const double max = 1.)
74 throw std::runtime_error(
"TODO quaternion spline does not implement derivate");
80 waypoints.push_back(std::make_pair(0, point_one_dim_t::Zero()));
81 waypoints.push_back(std::make_pair(1, point_one_dim_t::Ones()));
85 virtual time_t
min()
const {
return min_; }
86 virtual time_t
max()
const {
return max_; }
96 typedef exact_cubic<Time, Numeric, 4, false, quat_t, std::vector<quat_t, Eigen::aligned_allocator<quat_t> >,
126 template <
typename In>
129 const Point& lift_normal = Eigen::Vector3d::UnitZ(),
130 const Point& land_normal = Eigen::Vector3d::UnitZ(),
const Numeric lift_offset = 0.02,
131 const Numeric land_offset = 0.02,
const Time lift_offset_duration = 0.02,
132 const Time land_offset_duration = 0.02)
133 :
spline_(
effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, land_normal, lift_offset, land_offset,
134 lift_offset_duration, land_offset_duration)),
161 template <
typename In,
typename InQuat>
163 const Point& lift_normal = Eigen::Vector3d::UnitZ(),
164 const Point& land_normal = Eigen::Vector3d::UnitZ(),
const Numeric lift_offset = 0.02,
165 const Numeric land_offset = 0.02,
const Time lift_offset_duration = 0.02,
166 const Time land_offset_duration = 0.02)
167 :
spline_(
effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, land_normal, lift_offset, land_offset,
168 lift_offset_duration, land_offset_duration)),
169 to_quat_((quatWayPointsBegin->second).data()),
170 land_quat_(((quatWayPointsEnd - 1)->second).data()),
173 quat_spline_(quat_spline(quatWayPointsBegin, quatWayPointsEnd)) {
209 std::vector<rotation_spline> splines;
214 template <
typename InQuat>
215 exact_cubic_quat_t quat_spline(InQuat quatWayPointsBegin, InQuat quatWayPointsEnd)
const {
216 if (std::distance(quatWayPointsBegin, quatWayPointsEnd) < 1)
return simple_quat_spline();
218 InQuat it(quatWayPointsBegin);
220 Eigen::Quaterniond current_quat =
to_quat_;
221 for (; it != quatWayPointsEnd; ++it) {
222 splines.push_back(rotation_spline(current_quat.coeffs(), it->second, init, it->first));
223 current_quat = it->second;
245 #endif //_CLASS_EFFECTOR_SPLINE_ROTATION
config_t operator()(const Numeric t) const
Evaluation of the effector position and rotation at time t.
Definition: effector_spline_rotation.h:193
Numeric max() const
Definition: effector_spline_rotation.h:183
~effector_spline_rotation()
Definition: effector_spline_rotation.h:177
Numeric min() const
Definition: effector_spline_rotation.h:182
const double time_lift_offset_
Definition: effector_spline_rotation.h:237
curve_abc< Time, Numeric, 4, false, quat_t > curve_abc_quat_t
Definition: effector_spline_rotation.h:33
quat_t interpolate_quat(const Numeric t) const
Definition: effector_spline_rotation.h:201
spline_deriv_constraint_one_dim time_reparam_
Definition: effector_spline_rotation.h:93
const Eigen::Quaterniond to_quat_
Definition: effector_spline_rotation.h:235
std::vector< waypoint_quat_t > t_waypoint_quat_t
Definition: effector_spline_rotation.h:35
std::vector< waypoint_one_dim_t > t_waypoint_one_dim_t
Definition: effector_spline_rotation.h:39
std::pair< Numeric, quat_t > waypoint_quat_t
Definition: effector_spline_rotation.h:34
Eigen::Ref< quat_t > quat_ref_t
Definition: effector_spline_rotation.h:30
Eigen::Quaterniond quat_from_
Definition: effector_spline_rotation.h:89
spline_deriv_constraint< Numeric, Numeric, 1, false, point_one_dim_t > spline_deriv_constraint_one_dim
Definition: effector_spline_rotation.h:37
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:127
Eigen::Matrix< Numeric, 7, 1 > config_t
Definition: effector_spline_rotation.h:32
quat_t operator()(const Numeric t) const
Definition: effector_spline_rotation.h:64
const Eigen::Quaterniond land_quat_
Definition: effector_spline_rotation.h:236
~rotation_spline()
Definition: effector_spline_rotation.h:52
Represents a trajectory for and end effector uses the method effector_spline to create a spline traje...
Definition: effector_spline_rotation.h:107
Eigen::Quaterniond quat_to_
Definition: effector_spline_rotation.h:90
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:43
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:162
rotation_spline & operator=(const rotation_spline &from)
Definition: effector_spline_rotation.h:55
virtual time_t min() const
Definition: effector_spline_rotation.h:85
Eigen::Matrix< Numeric, 1, 1 > point_one_dim_t
Definition: effector_spline_rotation.h:36
double Numeric
Definition: effector_spline.h:26
double Time
Definition: effector_spline.h:27
const exact_cubic_t * spline_
Definition: effector_spline_rotation.h:234
virtual time_t max() const
Definition: effector_spline_rotation.h:86
spline_deriv_constraint_one_dim computeWayPoints() const
Definition: effector_spline_rotation.h:77
const double time_land_offset_
Definition: effector_spline_rotation.h:238
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:90
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28
exact_cubic< Time, Numeric, 4, false, quat_t, std::vector< quat_t, Eigen::aligned_allocator< quat_t > >, rotation_spline > exact_cubic_quat_t
Definition: effector_spline_rotation.h:98
spline_deriv_constraint_t::t_spline_t t_spline_t
Definition: effector_spline.h:35
virtual quat_t derivate(time_t, std::size_t) const
Definition: effector_spline_rotation.h:73
std::pair< Numeric, point_one_dim_t > waypoint_one_dim_t
Definition: effector_spline_rotation.h:38
const typedef Eigen::Ref< const quat_t > quat_ref_const_t
Definition: effector_spline_rotation.h:31
Eigen::Matrix< Numeric, 4, 1 > quat_t
Definition: effector_spline_rotation.h:29
spline_deriv_constraint_t::exact_cubic_t exact_cubic_t
Definition: effector_spline.h:34
Definition: effector_spline_rotation.h:41
double max_
Definition: effector_spline_rotation.h:92
Definition: bernstein.h:20
double min_
Definition: effector_spline_rotation.h:91
const exact_cubic_quat_t quat_spline_
Definition: effector_spline_rotation.h:239