1 #ifdef CURVES_WITH_PINOCCHIO_SUPPORT
3 #ifndef NDCURVES_SO3_SMOOTH_H
4 #define NDCURVES_SO3_SMOOTH_H
6 #include <Eigen/Geometry>
7 #include <boost/math/constants/constants.hpp>
8 #include <boost/serialization/split_free.hpp>
9 #include <boost/serialization/vector.hpp>
10 #include <pinocchio/fwd.hpp>
11 #include <pinocchio/spatial/explog.hpp>
24 template <
typename Time =
double,
typename Numeric = Time,
bool Safe = false>
25 struct SO3Smooth :
public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t> {
31 typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t>
curve_abc_t;
32 typedef constant_curve<Time, Numeric, Safe, point_derivate_t>
35 typedef polynomial<Time, Numeric, Safe, point1_t> min_jerk_t;
44 init_rot_(point_t::Identity()),
45 end_rot_(point_t::Identity()),
50 rot_diff_(pinocchio::log3(init_rot_.transpose() * end_rot_)),
55 const time_t t_min,
const time_t t_max)
64 tmp_init_quat.normalized();
65 tmp_end_quat.normalized();
66 init_rot_ = tmp_init_quat.toRotationMatrix();
67 end_rot_ = tmp_end_quat.toRotationMatrix();
68 rot_diff_ = pinocchio::log3(init_rot_.transpose() * end_rot_);
75 const time_t t_min,
const time_t t_max)
83 rot_diff_(pinocchio::log3(init_rot_.transpose() * end_rot_)),
97 quaternion_t init_rot_normalized = init_rot.normalized();
99 init_rot_ = init_rot_normalized.toRotationMatrix();
100 end_rot_ = end_rot_normalized.toRotationMatrix();
101 rot_diff_ = pinocchio::log3(init_rot_.transpose() * end_rot_);
116 rot_diff_ = pinocchio::log3(init_rot_.transpose() * end_rot_);
127 init_rot_.setIdentity();
128 end_rot_.setIdentity();
133 rot_diff_ = pinocchio::log3(init_rot_.transpose() * end_rot_);
139 const time_t t_min,
const time_t t_max) {
140 init_rot_ = init_rot;
146 rot_diff_ = pinocchio::log3(init_rot_.transpose() * end_rot_);
153 const time_t t_min,
const time_t t_max) {
154 quaternion_t init_rot_normalized = init_rot.normalized();
156 generate(init_rot_normalized.toRotationMatrix(),
157 end_rot_normalized.toRotationMatrix(), t_min, t_max);
161 quaternion_t init_rot_normalized = init_rot.normalized();
163 generate(init_rot_normalized.toRotationMatrix(),
164 end_rot_normalized.toRotationMatrix(), 0.0, 1.0);
170 generate(init_rot, end_rot, 0.0, 1.0);
174 virtual ~SO3Smooth() {}
177 SO3Smooth(
const SO3Smooth& other)
179 init_rot_(other.init_rot_),
180 end_rot_(other.end_rot_),
181 t_min_(other.t_min_),
182 t_max_(other.t_max_),
183 min_jerk_(other.min_jerk_),
184 rot_diff_(other.rot_diff_),
190 virtual point_t operator()(
const time_t t)
const {
191 if ((t < t_min_ || t > t_max_) && Safe) {
192 throw std::invalid_argument(
193 "error in SO3Smooth : time t to evaluate derivative should be in "
194 "range [Tmin, Tmax] of the curve");
196 return init_rot_ * pinocchio::exp3(min_jerk_(t)[0] * rot_diff_);
210 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
211 return ndcurves::isApprox<Numeric>(t_min_, other.min()) &&
212 ndcurves::isApprox<Numeric>(t_max_, other.max()) &&
213 init_rot_.isApprox(other.init_rot_, prec) &&
214 end_rot_.isApprox(other.end_rot_, prec);
219 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
227 virtual bool operator==(
const SO3Smooth_t& other)
const {
231 virtual bool operator!=(
const SO3Smooth_t& other)
const {
232 return !(*
this == other);
246 virtual point_derivate_t derivate(
const time_t t,
247 const std::size_t order)
const {
248 point_derivate_t out = point_derivate_t::Zero();
249 if ((t < t_min_ || t > t_max_) && Safe) {
250 throw std::invalid_argument(
251 "error in SO3Smooth : time t to evaluate derivative should be in "
252 "range [Tmin, Tmax] of the curve");
255 throw std::invalid_argument(
"Order must be in {1, 2}.");
256 }
else if (order == 2) {
270 out = (derivate(t1, 1) - derivate(t2, 1)) / (t2 - t1);
272 }
else if (order == 1) {
274 pinocchio::Jexp3(min_jerk_(t)[0] * rot_diff_, jexp);
276 init_rot_ * jexp * min_jerk_.derivate(t, 1)[0] * rot_diff_;
278 throw std::invalid_argument(
"Order must be > 0 ");
283 curve_derivate_t compute_derivate(
const std::size_t )
const {
284 throw std::runtime_error(
"This function has not been implemented yet.");
291 curve_derivate_t* compute_derivate_ptr(
const std::size_t order)
const {
292 return new curve_derivate_t(compute_derivate(order));
298 std::size_t
virtual dim()
const {
return 3; };
301 time_t min()
const {
return t_min_; }
304 time_t max()
const {
return t_max_; }
307 virtual std::size_t degree()
const {
return 5; }
324 friend class boost::serialization::access;
326 template <
class Archive>
327 void load(Archive& ar,
const unsigned int version) {
331 ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(
curve_abc_t);
332 ar >> boost::serialization::make_nvp(
"init_rotation", init_rot_);
333 ar >> boost::serialization::make_nvp(
"end_rotation", end_rot_);
334 ar >> boost::serialization::make_nvp(
"t_min", t_min_);
335 ar >> boost::serialization::make_nvp(
"t_max", t_max_);
336 ar >> boost::serialization::make_nvp(
"min_jerk", min_jerk_);
337 ar >> boost::serialization::make_nvp(
"rot_diff", rot_diff_);
340 template <
class Archive>
341 void save(Archive& ar,
const unsigned int version)
const {
345 ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(
curve_abc_t);
346 ar << boost::serialization::make_nvp(
"init_rotation", init_rot_);
347 ar << boost::serialization::make_nvp(
"end_rotation", end_rot_);
348 ar << boost::serialization::make_nvp(
"t_min", t_min_);
349 ar << boost::serialization::make_nvp(
"t_max", t_max_);
350 ar << boost::serialization::make_nvp(
"min_jerk", min_jerk_);
351 ar << boost::serialization::make_nvp(
"rot_diff", rot_diff_);
354 BOOST_SERIALIZATION_SPLIT_MEMBER()
367 static const Scalar PI_value = boost::math::constants::pi<Scalar>();
370 const Scalar tr = R.trace();
373 else if (tr < Scalar(-1))
376 theta = acos((tr - Scalar(1)) / Scalar(2));
377 if (!std::isfinite(theta)) {
378 throw std::runtime_error(
"theta contains some NaN");
382 if (theta < PI_value - 1e-2) {
385 std::pow(std::numeric_limits<Scalar>::epsilon(),
391 res(0) = t * (R(2, 1) - R(1, 2));
392 res(1) = t * (R(0, 2) - R(2, 0));
393 res(2) = t * (R(1, 0) - R(0, 1));
399 const Scalar cphi = cos(theta - PI_value);
400 const Scalar beta = theta * theta / (Scalar(1) + cphi);
401 point3_t tmp((R.diagonal().array() + cphi) * beta);
402 res(0) = (R(2, 1) > R(1, 2) ? Scalar(1) : Scalar(-1)) *
403 (tmp[0] > Scalar(0) ? sqrt(tmp[0]) : Scalar(0));
404 res(1) = (R(0, 2) > R(2, 0) ? Scalar(1) : Scalar(-1)) *
405 (tmp[1] > Scalar(0) ? sqrt(tmp[1]) : Scalar(0));
406 res(2) = (R(1, 0) > R(0, 1) ? Scalar(1) : Scalar(-1)) *
407 (tmp[2] > Scalar(0) ? sqrt(tmp[2]) : Scalar(0));
416 if (t_min_ > t_max_) {
417 throw std::invalid_argument(
"Tmin should be inferior to Tmax");
#define DEFINE_CLASS_TEMPLATE_VERSION(Template, Type)
Definition: archive.hpp:27
#define SINGLE_ARG(...)
Definition: archive.hpp:23
class allowing to create a constant_curve curve.
interface for a Curve of arbitrary dimension.
void load(Archive &ar, Eigen::Matrix< _Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols > &m, const unsigned int)
Definition: eigen-matrix.hpp:63
void save(Archive &ar, const Eigen::Matrix< _Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols > &m, const unsigned int)
Definition: eigen-matrix.hpp:50
double Numeric
Definition: effector_spline.h:26
double Time
Definition: effector_spline.h:27
Definition: bernstein.h:20
Eigen::Matrix< double, 1, 1 > point1_t
Definition: fwd.h:66
Eigen::Ref< const matrix3_t > matrix3_t_cst_ref
Definition: fwd.h:81
curve_abc< double, double, true, pointX_t, pointX_t > curve_abc_t
Definition: fwd.h:85
Eigen::Vector3d point3_t
Definition: fwd.h:71
Eigen::Quaternion< double > quaternion_t
Definition: fwd.h:76
Eigen::Matrix< double, 3, 3 > matrix3_t
Definition: fwd.h:74
bool isApprox(const T a, const T b, const T eps=1e-6)
Definition: curve_abc.h:26
SO3Smooth< double, double, true > SO3Smooth_t
Definition: fwd.h:131
Definition of a cubic spline.