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)
99 return point3_t::Zero();
101 return log3(init_rot.transpose() * end_rot) / (t_max - t_min);
107 throw std::invalid_argument(
"can't evaluate bezier curve, time t is out of range");
128 bool isApprox(
const SO3Linear_t& other,
const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
129 return ndcurves::isApprox<Numeric>(
T_min_, other.
min()) && ndcurves::isApprox<Numeric>(
T_max_, other.
max()) &&
131 end_rot_.toRotationMatrix().isApprox(other.
end_rot_.toRotationMatrix(), prec);
135 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
136 const SO3Linear_t* other_cast =
dynamic_cast<const SO3Linear_t*
>(other);
145 virtual bool operator!=(
const SO3Linear_t& other)
const {
return !(*
this == other); }
151 virtual point_derivate_t
derivate(
const time_t t,
const std::size_t order)
const {
152 if ((t < T_min_ || t >
T_max_) && Safe) {
153 throw std::invalid_argument(
154 "error in SO3_linear : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve");
156 if (order > 1 || t > T_max_ || t <
T_min_) {
157 return point3_t::Zero(3);
158 }
else if (order == 1) {
161 throw std::invalid_argument(
"Order must be > 0 ");
177 std::size_t
virtual dim()
const {
return dim_; };
186 virtual std::size_t
degree()
const {
return 1; }
204 template <
class Archive>
205 void load(Archive& ar,
const unsigned int version) {
209 ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
210 ar >> boost::serialization::make_nvp(
"dim", dim_);
212 ar >> boost::serialization::make_nvp(
"init_rotation", init);
213 ar >> boost::serialization::make_nvp(
"end_rotation", end);
216 ar >> boost::serialization::make_nvp(
"angular_vel", angular_vel_);
217 ar >> boost::serialization::make_nvp(
"T_min", T_min_);
218 ar >> boost::serialization::make_nvp(
"T_max", T_max_);
221 template <
class Archive>
222 void save(Archive& ar,
const unsigned int version)
const {
226 ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
227 ar << boost::serialization::make_nvp(
"dim", dim_);
230 ar << boost::serialization::make_nvp(
"init_rotation", init_matrix);
231 ar << boost::serialization::make_nvp(
"end_rotation", end_matrix);
232 ar << boost::serialization::make_nvp(
"angular_vel", angular_vel_);
233 ar << boost::serialization::make_nvp(
"T_min", T_min_);
234 ar << boost::serialization::make_nvp(
"T_max", T_max_);
237 BOOST_SERIALIZATION_SPLIT_MEMBER()
250 static const Scalar PI_value = boost::math::constants::pi<Scalar>();
253 const Scalar tr = R.trace();
260 if (!std::isfinite(theta)) {
261 throw std::runtime_error(
"theta contains some NaN");
265 if (theta < PI_value - 1e-2) {
266 const Scalar t = ((theta > std::pow(std::numeric_limits<Scalar>::epsilon(),
271 res(0) = t * (R(2, 1) - R(1, 2));
272 res(1) = t * (R(0, 2) - R(2, 0));
273 res(2) = t * (R(1, 0) - R(0, 1));
279 const Scalar cphi = cos(theta - PI_value);
280 const Scalar beta = theta * theta / (
Scalar(1) + cphi);
281 point3_t tmp((R.diagonal().array() + cphi) * beta);
293 if (T_min_ > T_max_) {
294 throw std::invalid_argument(
"Tmin should be inferior to Tmax");
303 DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(
typename Time,
typename Numeric,
bool Safe),
306 #endif // _STRUCT_SO3_LINEAR_H Definition: bernstein.h:20
time_t T_min_
Definition: so3_linear.h:198
virtual ~SO3Linear()
Destructor.
Definition: so3_linear.h:87
SO3Linear< Time, Numeric, Safe > SO3Linear_t
Definition: so3_linear.h:29
virtual bool operator==(const SO3Linear_t &other) const
Definition: so3_linear.h:143
quaternion_t init_rot_
Definition: so3_linear.h:196
virtual bool operator!=(const SO3Linear_t &other) const
Definition: so3_linear.h:145
matrix3_t point_t
Definition: so3_linear.h:23
virtual point_t operator()(const time_t t) const
Evaluation of the SO3Linear at time t using Eigen slerp.
Definition: so3_linear.h:118
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:151
interface for a Curve of arbitrary dimension.
Eigen::Vector3d point3_t
Definition: fwd.h:58
virtual std::size_t degree() const
Get the degree of the curve.
Definition: so3_linear.h:186
matrix3_t getInitRotation() const
Definition: so3_linear.h:187
matrix3_t getInitRotation()
Definition: so3_linear.h:189
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
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:134
matrix3_t getEndRotation()
Definition: so3_linear.h:190
time_t T_max_
Definition: so3_linear.h:198
SO3Linear()
Empty constructor. Curve obtained this way can not perform other class functions. ...
Definition: so3_linear.h:36
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:97
double Time
Definition: effector_spline.h:27
constant_curve< Time, Numeric, Safe, point_derivate_t > curve_derivate_t
Definition: so3_linear.h:28
curve_abc< Time, Numeric, Safe, point_t, point_derivate_t > curve_abc_t
Definition: so3_linear.h:27
Eigen::Matrix< double, 3, 3 > matrix3_t
Definition: fwd.h:65
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
Numeric Scalar
Definition: so3_linear.h:22
curve_derivate_t compute_derivate(const std::size_t order) const
Definition: so3_linear.h:165
std::size_t dim_
Definition: so3_linear.h:195
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
time_t min() const
Get the minimum time for which the curve is defined.
Definition: so3_linear.h:180
friend class boost::serialization::access
Definition: so3_linear.h:202
quaternion_t computeAsQuaternion(const time_t t) const
Definition: so3_linear.h:105
Time time_t
Definition: so3_linear.h:26
Eigen::Quaternion< Scalar > quaternion_t
Definition: so3_linear.h:25
matrix3_t getEndRotation() const
Definition: so3_linear.h:188
double Numeric
Definition: effector_spline.h:26
void save(Archive &ar, const unsigned int version) const
Definition: so3_linear.h:222
Represents a linear interpolation in SO3, using the slerp method provided by Eigen::Quaternion.
Definition: fwd.h:46
time_t max() const
Get the maximum time for which the curve is defined.
Definition: so3_linear.h:183
SO3Linear(const SO3Linear &other)
Definition: so3_linear.h:89
virtual std::size_t dim() const
Get dimension of curve.
Definition: so3_linear.h:177
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:128
point3_t point_derivate_t
Definition: so3_linear.h:24
point3_t log3(const matrix3_t &R)
Log: SO3 -> so3.
Definition: so3_linear.h:248
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
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:34
class allowing to create a constant_curve curve.
point3_t angular_vel_
Definition: so3_linear.h:197
Represents a constant_curve curve, always returning the same value and a null derivative.
Definition: constant_curve.h:20
curve_derivate_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: so3_linear.h:172
void load(Archive &ar, const unsigned int version)
Definition: so3_linear.h:205
quaternion_t end_rot_
Definition: so3_linear.h:196