1 #ifndef _STRUCT_SO3_LINEAR_H 2 #define _STRUCT_SO3_LINEAR_H 8 #include <Eigen/Geometry> 9 #include <boost/math/constants/constants.hpp> 11 #include <boost/serialization/split_free.hpp> 12 #include <boost/serialization/vector.hpp> 20 template <
typename Time =
double,
typename Numeric = Time,
bool Safe = false>
21 struct SO3Linear :
public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
39 SO3Linear(
const quaternion_t& init_rot,
const quaternion_t& end_rot,
const time_t t_min,
const time_t t_max)
63 SO3Linear(
const quaternion_t& init_rot,
const quaternion_t& end_rot)
101 return point3_t::Zero();
103 return log3(init_rot.transpose() * end_rot) / (t_max - t_min);
109 throw std::invalid_argument(
"can't evaluate bezier curve, time t is out of range");
130 bool isApprox(
const SO3Linear_t& other,
const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
131 return curves::isApprox<Numeric>(
T_min_, other.
min()) && curves::isApprox<Numeric>(
T_max_, other.
max()) &&
133 end_rot_.toRotationMatrix().isApprox(other.
end_rot_.toRotationMatrix(), prec);
137 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
138 const SO3Linear_t* other_cast =
dynamic_cast<const SO3Linear_t*
>(other);
147 virtual bool operator!=(
const SO3Linear_t& other)
const {
return !(*
this == other); }
153 virtual point_derivate_t
derivate(
const time_t t,
const std::size_t order)
const {
154 if ((t < T_min_ || t >
T_max_) && Safe) {
155 throw std::invalid_argument(
156 "error in SO3_linear : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve");
158 if (order > 1 || t > T_max_ || t <
T_min_) {
159 return point3_t::Zero(3);
160 }
else if (order == 1) {
163 throw std::invalid_argument(
"Order must be > 0 ");
179 std::size_t
virtual dim()
const {
return dim_; };
188 virtual std::size_t
degree()
const {
return 1; }
206 template <
class Archive>
207 void load(Archive& ar,
const unsigned int version) {
211 ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
212 ar >> boost::serialization::make_nvp(
"dim", dim_);
214 ar >> boost::serialization::make_nvp(
"init_rotation", init);
215 ar >> boost::serialization::make_nvp(
"end_rotation", end);
218 ar >> boost::serialization::make_nvp(
"angular_vel", angular_vel_);
219 ar >> boost::serialization::make_nvp(
"T_min", T_min_);
220 ar >> boost::serialization::make_nvp(
"T_max", T_max_);
223 template <
class Archive>
224 void save(Archive& ar,
const unsigned int version)
const {
228 ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
229 ar << boost::serialization::make_nvp(
"dim", dim_);
232 ar << boost::serialization::make_nvp(
"init_rotation", init_matrix);
233 ar << boost::serialization::make_nvp(
"end_rotation", end_matrix);
234 ar << boost::serialization::make_nvp(
"angular_vel", angular_vel_);
235 ar << boost::serialization::make_nvp(
"T_min", T_min_);
236 ar << boost::serialization::make_nvp(
"T_max", T_max_);
239 BOOST_SERIALIZATION_SPLIT_MEMBER()
252 static const Scalar PI_value = boost::math::constants::pi<Scalar>();
255 const Scalar tr = R.trace();
262 if (!std::isfinite(theta)) {
263 throw std::runtime_error(
"theta contains some NaN");
267 if (theta < PI_value - 1e-2) {
268 const Scalar t = ((theta > std::pow(std::numeric_limits<Scalar>::epsilon(),
273 res(0) = t * (R(2, 1) - R(1, 2));
274 res(1) = t * (R(0, 2) - R(2, 0));
275 res(2) = t * (R(1, 0) - R(0, 1));
281 const Scalar cphi = cos(theta - PI_value);
282 const Scalar beta = theta * theta / (
Scalar(1) + cphi);
283 point3_t tmp((R.diagonal().array() + cphi) * beta);
295 if (T_min_ > T_max_) {
296 throw std::invalid_argument(
"Tmin should be inferior to Tmax");
305 DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(
typename Time,
typename Numeric,
bool Safe),
308 #endif // _STRUCT_SO3_LINEAR_H matrix3_t getEndRotation()
Definition: so3_linear.h:192
point3_t angular_vel_
Definition: so3_linear.h:199
SO3Linear()
Empty constructor. Curve obtained this way can not perform other class functions. ...
Definition: so3_linear.h:36
double Numeric
Definition: effector_spline.h:26
virtual point_t operator()(const time_t t) const
Evaluation of the SO3Linear at time t using Eigen slerp.
Definition: so3_linear.h:120
time_t min() const
Get the minimum time for which the curve is defined.
Definition: so3_linear.h:182
Eigen::Quaternion< Scalar > quaternion_t
Definition: so3_linear.h:25
std::size_t dim_
Definition: so3_linear.h:197
Eigen::Vector3d point3_t
Definition: fwd.h:58
virtual bool operator!=(const SO3Linear_t &other) const
Definition: so3_linear.h:147
virtual std::size_t degree() const
Get the degree of the curve.
Definition: so3_linear.h:188
SO3Linear< Time, Numeric, Safe > SO3Linear_t
Definition: so3_linear.h:29
quaternion_t computeAsQuaternion(const time_t t) const
Definition: so3_linear.h:107
matrix3_t getEndRotation() const
Definition: so3_linear.h:190
SO3Linear(const matrix3_t &init_rot, const matrix3_t &end_rot)
constructor with initial and final rotation expressed as rotation matrix, time bounds are set to [0;1...
Definition: so3_linear.h:75
interface for a Curve of arbitrary dimension.
time_t T_min_
Definition: so3_linear.h:200
point3_t point_derivate_t
Definition: so3_linear.h:24
Definition: bernstein.h:20
void save(Archive &ar, const unsigned int version) const
Definition: so3_linear.h:224
curve_abc< Time, Numeric, Safe, point_t, point_derivate_t > curve_abc_t
Definition: so3_linear.h:27
void load(Archive &ar, const unsigned int version)
Definition: so3_linear.h:207
SO3Linear(const quaternion_t &init_rot, const quaternion_t &end_rot, const time_t t_min, const time_t t_max)
constructor with initial and final rotation and time bounds
Definition: so3_linear.h:39
matrix3_t point_t
Definition: so3_linear.h:23
friend class boost::serialization::access
Definition: so3_linear.h:204
quaternion_t end_rot_
Definition: so3_linear.h:198
virtual std::size_t dim() const
Get dimension of curve.
Definition: so3_linear.h:179
curve_derivate_t compute_derivate(const std::size_t order) const
Definition: so3_linear.h:167
Eigen::Matrix< double, 3, 3 > matrix3_t
Definition: fwd.h:65
SO3Linear(const quaternion_t &init_rot, const quaternion_t &end_rot)
constructor with initial and final rotation, time bounds are set to [0;1]
Definition: so3_linear.h:63
SO3Linear(const matrix3_t &init_rot, const matrix3_t &end_rot, const time_t t_min, const time_t t_max)
constructor with initial and final rotation expressed as rotation matrix and time bounds ...
Definition: so3_linear.h:51
Represents a constant_curve curve, always returning the same value and a null derivative.
Definition: constant_curve.h:20
point3_t computeAngularVelocity(const matrix3_t &init_rot, const matrix3_t &end_rot, const double t_min, const double t_max)
Definition: so3_linear.h:99
quaternion_t init_rot_
Definition: so3_linear.h:198
matrix3_t getInitRotation()
Definition: so3_linear.h:191
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equal given a precision treshold Only two curves ...
Definition: so3_linear.h:136
Numeric Scalar
Definition: so3_linear.h:22
SO3Linear(const SO3Linear &other)
Definition: so3_linear.h:91
Represents a linear interpolation in SO3, using the slerp method provided by Eigen::Quaternion.
Definition: fwd.h:46
point3_t log3(const matrix3_t &R)
Log: SO3 -> so3.
Definition: so3_linear.h:250
matrix3_t getInitRotation() const
Definition: so3_linear.h:189
constant_curve< Time, Numeric, Safe, point_derivate_t > curve_derivate_t
Definition: so3_linear.h:28
Time time_t
Definition: so3_linear.h:26
~SO3Linear()
Destructor.
Definition: so3_linear.h:87
double Time
Definition: effector_spline.h:27
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:34
virtual bool operator==(const SO3Linear_t &other) const
Definition: so3_linear.h:145
time_t max() const
Get the maximum time for which the curve is defined.
Definition: so3_linear.h:185
virtual point_derivate_t derivate(const time_t t, const std::size_t order) const
Evaluation of the derivative of order N of spline at time t.
Definition: so3_linear.h:153
curve_derivate_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: so3_linear.h:174
class allowing to create a constant_curve curve.
time_t T_max_
Definition: so3_linear.h:200
bool isApprox(const SO3Linear_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: so3_linear.h:130