Go to the documentation of this file. 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 > {
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");
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 {
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");
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);
281 point3_t tmp((R.diagonal().array() + cphi) * beta);
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
virtual bool operator!=(const SO3Linear_t &other) const
Definition: so3_linear.h:145
SO3Linear< Time, Numeric, Safe > SO3Linear_t
Definition: so3_linear.h:29
virtual point_t operator()(const time_t t) const
Evaluation of the SO3Linear at time t using Eigen slerp.
Definition: so3_linear.h:118
friend class boost::serialization::access
Definition: so3_linear.h:202
SO3Linear()
Empty constructor. Curve obtained this way can not perform other class functions.
Definition: so3_linear.h:36
quaternion_t end_rot_
Definition: so3_linear.h:196
time_t min() const
Get the minimum time for which the curve is defined.
Definition: so3_linear.h:180
matrix3_t getEndRotation() const
Definition: so3_linear.h:188
Eigen::Quaternion< Scalar > quaternion_t
Definition: so3_linear.h:25
virtual std::size_t dim() const
Get dimension of curve.
Definition: so3_linear.h:177
Represents a linear interpolation in SO3, using the slerp method provided by Eigen::Quaternion.
Definition: fwd.h:46
std::size_t dim_
Definition: so3_linear.h:195
constant_curve< Time, Numeric, Safe, point_derivate_t > curve_derivate_t
Definition: so3_linear.h:28
virtual ~SO3Linear()
Destructor.
Definition: so3_linear.h:87
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
Numeric Scalar
Definition: so3_linear.h:22
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
point3_t log3(const matrix3_t &R)
Log: SO3 -> so3.
Definition: so3_linear.h:248
matrix3_t getInitRotation()
Definition: so3_linear.h:189
class allowing to create a constant_curve curve.
curve_derivate_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: so3_linear.h:172
time_t max() const
Get the maximum time for which the curve is defined.
Definition: so3_linear.h:183
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
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:34
time_t T_max_
Definition: so3_linear.h:198
Definition: bernstein.h:20
double Time
Definition: effector_spline.h:27
point3_t angular_vel_
Definition: so3_linear.h:197
void save(Archive &ar, const unsigned int version) const
Definition: so3_linear.h:222
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
curve_derivate_t compute_derivate(const std::size_t order) const
Definition: so3_linear.h:165
time_t T_min_
Definition: so3_linear.h:198
interface for a Curve of arbitrary dimension.
Time time_t
Definition: so3_linear.h:26
matrix3_t getEndRotation()
Definition: so3_linear.h:190
quaternion_t computeAsQuaternion(const time_t t) const
Definition: so3_linear.h:105
Eigen::Vector3d point3_t
Definition: fwd.h:58
SO3Linear(const SO3Linear &other)
Definition: so3_linear.h:89
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:97
virtual bool operator==(const SO3Linear_t &other) const
Definition: so3_linear.h:143
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: so3_linear.h:134
matrix3_t getInitRotation() const
Definition: so3_linear.h:187
void load(Archive &ar, const unsigned int version)
Definition: so3_linear.h:205
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
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
double Numeric
Definition: effector_spline.h:26
virtual std::size_t degree() const
Get the degree of the curve.
Definition: so3_linear.h:186
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
matrix3_t point_t
Definition: so3_linear.h:23
quaternion_t init_rot_
Definition: so3_linear.h:196