Represents a trajectory for and end effector. More...
#include <curves/helpers/effector_spline_rotation.h>
Public Member Functions | |
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) | |
Constructor. More... | |
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) | |
Constructor. More... | |
~effector_spline_rotation () | |
Numeric | min () const |
Numeric | max () const |
config_t | operator() (const Numeric t) const |
Evaluation of the effector position and rotation at time t. More... | |
quat_t | interpolate_quat (const Numeric t) const |
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_ |
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.
|
inline |
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.
wayPointsBegin | : an iterator pointing to the first element of a waypoint container. |
wayPointsEnd | : an iterator pointing to the last element of a waypoint container. |
to_quat | : 4D vector, quaternion indicating rotation at take off(x, y, z, w). |
land_quat | : 4D vector, quaternion indicating rotation at landing (x, y, z, w). |
lift_normal | : normal to be followed by end effector at take-off. |
land_normal | : normal to be followed by end effector at landing. |
lift_offset | : length of the straight line along normal at take-off. |
land_offset | : length of the straight line along normal at landing. |
lift_offset_duration | : time travelled along straight line at take-off. |
land_offset_duration | : time travelled along straight line at landing. |
|
inline |
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.
wayPointsBegin | : an iterator pointing to the first element of a waypoint container. |
wayPointsEnd | : an iterator pointing to the last element of a waypoint container. |
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. |
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. |
lift_normal | : normal to be followed by end effector at take-off. |
land_normal | : normal to be followed by end effector at landing. |
lift_offset | : length of the straight line along normal at take-off. |
land_offset | : length of the straight line along normal at landing. |
lift_offset_duration | : time travelled along straight line at take-off. |
land_offset_duration | : time travelled along straight line at landing. |
|
inline |
|
inline |
|
inline |
Evaluation of the effector position and rotation at time t.
t | : the time when to evaluate the spline. |
const Eigen::Quaterniond curves::helpers::effector_spline_rotation::land_quat_ |
const exact_cubic_quat_t curves::helpers::effector_spline_rotation::quat_spline_ |
const exact_cubic_t* curves::helpers::effector_spline_rotation::spline_ |
const double curves::helpers::effector_spline_rotation::time_land_offset_ |
const double curves::helpers::effector_spline_rotation::time_lift_offset_ |
const Eigen::Quaterniond curves::helpers::effector_spline_rotation::to_quat_ |