6 #ifndef __pinocchio_spatial_explog_hpp__
7 #define __pinocchio_spatial_explog_hpp__
18 #include <Eigen/Geometry>
32 template<
typename Vector3Like>
40 typedef Eigen::Matrix<Scalar,3,3,Vector3LikePlain::Options> Matrix3;
42 const Scalar t2 =
v.squaredNorm();
44 const Scalar t = math::sqrt(t2);
53 Matrix3
res(alpha_vxvx *
v *
v.transpose());
54 res.coeffRef(0,1) -= alpha_vx *
v[2];
res.coeffRef(1,0) += alpha_vx *
v[2];
55 res.coeffRef(0,2) += alpha_vx *
v[1];
res.coeffRef(2,0) -= alpha_vx *
v[1];
56 res.coeffRef(1,2) -= alpha_vx *
v[0];
res.coeffRef(2,1) += alpha_vx *
v[0];
61 res.diagonal().array() += ct;
73 template<
typename Matrix3Like>
76 typename Matrix3Like::
Scalar & theta)
79 typedef Eigen::Matrix<
Scalar,3,1,
94 template<
typename Matrix3Like>
99 return log3(
R.derived(),theta);
110 template<AssignmentOperatorType op,
typename Vector3Like,
typename Matrix3Like>
111 void Jexp3(
const Eigen::MatrixBase<Vector3Like> &
r,
112 const Eigen::MatrixBase<Matrix3Like> & Jexp)
120 const Scalar n2 =
r.squaredNorm();
121 const Scalar n = math::sqrt(n2);
123 const Scalar n2_inv = n_inv * n_inv;
139 Jout.diagonal().setConstant(
a);
140 Jout(0,1) = -
b*
r[2]; Jout(1,0) = -Jout(0,1);
141 Jout(0,2) =
b*
r[1]; Jout(2,0) = -Jout(0,2);
142 Jout(1,2) = -
b*
r[0]; Jout(2,1) = -Jout(1,2);
143 Jout.noalias() +=
c *
r *
r.transpose();
146 Jout.diagonal().array() +=
a;
147 Jout(0,1) += -
b*
r[2]; Jout(1,0) +=
b*
r[2];
148 Jout(0,2) +=
b*
r[1]; Jout(2,0) += -
b*
r[1];
149 Jout(1,2) += -
b*
r[0]; Jout(2,1) +=
b*
r[0];
150 Jout.noalias() +=
c *
r *
r.transpose();
153 Jout.diagonal().array() -=
a;
154 Jout(0,1) -= -
b*
r[2]; Jout(1,0) -=
b*
r[2];
155 Jout(0,2) -=
b*
r[1]; Jout(2,0) -= -
b*
r[1];
156 Jout(1,2) -= -
b*
r[0]; Jout(2,1) -=
b*
r[0];
157 Jout.noalias() -=
c *
r *
r.transpose();
160 assert(
false &&
"Wrong Op requesed value");
173 template<
typename Vector3Like,
typename Matrix3Like>
174 void Jexp3(
const Eigen::MatrixBase<Vector3Like> &
r,
175 const Eigen::MatrixBase<Matrix3Like> & Jexp)
177 Jexp3<SETTO>(
r, Jexp);
222 template<
typename Scalar,
typename Vector3Like,
typename Matrix3Like>
224 const Eigen::MatrixBase<Vector3Like> &
log,
225 const Eigen::MatrixBase<Matrix3Like> & Jlog)
243 template<
typename Matrix3Like1,
typename Matrix3Like2>
244 void Jlog3(
const Eigen::MatrixBase<Matrix3Like1> & R,
245 const Eigen::MatrixBase<Matrix3Like2> & Jlog)
255 template<
typename Scalar,
typename Vector3Like1,
typename Vector3Like2,
typename Matrix3Like>
257 const Eigen::MatrixBase<Vector3Like1> &
log,
258 const Eigen::MatrixBase<Vector3Like2> &
v,
259 const Eigen::MatrixBase<Matrix3Like> & vt_Hlog)
270 Scalar denom = .5 / (1-ctheta),
271 a = theta * stheta * denom,
272 da_dt = (stheta - theta) * denom,
273 b = ( 1 -
a ) / (theta*theta),
275 db_dt = - (2 / theta - (theta + stheta) * denom) / (theta*theta);
280 Vector3 dl_dv_v (
a*
v + .5*
log.cross(
v) +
b*
log*
log.transpose()*
v);
282 Scalar dt_dv_v =
log.dot(dl_dv_v) / theta;
285 vt_Hlog_.noalias() = db_dt * dt_dv_v *
log *
log.transpose();
286 vt_Hlog_.noalias() +=
b * dl_dv_v *
log.transpose();
287 vt_Hlog_.noalias() +=
b *
log * dl_dv_v.transpose();
289 addSkew(.5 * dl_dv_v, vt_Hlog_);
291 vt_Hlog_.diagonal().array() += da_dt * dt_dv_v;
302 template<
typename Matrix3Like1,
typename Vector3Like,
typename Matrix3Like2>
303 void Hlog3(
const Eigen::MatrixBase<Matrix3Like1> & R,
304 const Eigen::MatrixBase<Vector3Like> &
v,
305 const Eigen::MatrixBase<Matrix3Like2> & vt_Hlog)
324 template<
typename MotionDerived>
334 typename SE3::LinearType & trans =
res.translation();
335 typename SE3::AngularType & rot =
res.rotation();
337 const typename MotionDerived::ConstAngularType &
w = nu.angular();
338 const typename MotionDerived::ConstLinearType &
v = nu.linear();
340 Scalar alpha_wxv, alpha_v, alpha_w, diagonal_term;
341 const Scalar t2 =
w.squaredNorm();
342 const Scalar t = math::sqrt(t2);
356 (
Scalar(1) - alpha_v)*inv_t2);
363 trans.noalias() = (alpha_v*
v + (alpha_w*
w.dot(
v))*
w + alpha_wxv*
w.cross(
v));
366 rot.noalias() = alpha_wxv *
w *
w.transpose();
367 rot.coeffRef(0,1) -= alpha_v *
w[2]; rot.coeffRef(1,0) += alpha_v *
w[2];
368 rot.coeffRef(0,2) += alpha_v *
w[1]; rot.coeffRef(2,0) -= alpha_v *
w[1];
369 rot.coeffRef(1,2) -= alpha_v *
w[0]; rot.coeffRef(2,1) += alpha_v *
w[0];
370 rot.diagonal().array() += diagonal_term;
383 template<
typename Vector6Like>
401 template<
typename Scalar,
int Options>
402 MotionTpl<Scalar,Options>
419 template<
typename Matrix4Like>
421 log6(
const Eigen::MatrixBase<Matrix4Like> & M)
438 template<AssignmentOperatorType op,
typename MotionDerived,
typename Matrix6Like>
440 const Eigen::MatrixBase<Matrix6Like> & Jexp)
445 typedef typename MotionDerived::Vector3 Vector3;
446 typedef Eigen::Matrix<Scalar, 3, 3, Vector3::Options> Matrix3;
449 const typename MotionDerived::ConstLinearType &
v = nu.
linear();
450 const typename MotionDerived::ConstAngularType &
w = nu.
angular();
451 const Scalar t2 =
w.squaredNorm();
452 const Scalar t = math::sqrt(t2);
462 t2inv - st*tinv*inv_2_2ct);
466 -
Scalar(2)*t2inv*t2inv + (
Scalar(1) + st*tinv) * t2inv * inv_2_2ct);
472 Jexp3<SETTO>(
w, Jout.template bottomRightCorner<3,3>());
473 Jout.template topLeftCorner<3,3>() = Jout.template bottomRightCorner<3,3>();
474 const Vector3
p = Jout.template topLeftCorner<3,3>().transpose() *
v;
477 (beta_dot_over_theta*wTp) *
w*
w.transpose()
478 - (t2*beta_dot_over_theta+
Scalar(2)*beta)*
p*
w.transpose()
479 + wTp * beta * Matrix3::Identity()
480 + beta *
w*
p.transpose());
481 Jout.template topRightCorner<3,3>().noalias() =
482 - Jout.template topLeftCorner<3,3>() *
J;
483 Jout.template bottomLeftCorner<3,3>().setZero();
491 Jexp3<SETTO>(
w, Jtmp3);
493 Jout.template bottomRightCorner<3,3>() += Jtmp3;
494 Jout.template topLeftCorner<3,3>() += Jtmp3;
495 const Vector3
p = Jtmp3.transpose() *
v;
498 (beta_dot_over_theta*wTp) *
w*
w.transpose()
499 - (t2*beta_dot_over_theta+
Scalar(2)*beta)*
p*
w.transpose()
500 + wTp * beta * Matrix3::Identity()
501 + beta *
w*
p.transpose());
502 Jout.template topRightCorner<3,3>().noalias() +=
511 Jexp3<SETTO>(
w, Jtmp3);
513 Jout.template bottomRightCorner<3,3>() -= Jtmp3;
514 Jout.template topLeftCorner<3,3>() -= Jtmp3;
515 const Vector3
p = Jtmp3.transpose() *
v;
518 (beta_dot_over_theta*wTp) *
w*
w.transpose()
519 - (t2*beta_dot_over_theta+
Scalar(2)*beta)*
p*
w.transpose()
520 + wTp * beta * Matrix3::Identity()
521 + beta *
w*
p.transpose());
522 Jout.template topRightCorner<3,3>().noalias() -=
527 assert(
false &&
"Wrong Op requesed value");
534 template<
typename MotionDerived,
typename Matrix6Like>
536 const Eigen::MatrixBase<Matrix6Like> & Jexp)
538 Jexp6<SETTO>(nu, Jexp);
599 template<
typename Scalar,
int Options,
typename Matrix6Like>
601 const Eigen::MatrixBase<Matrix6Like> & Jlog)
606 template<
typename Scalar,
int Options>
607 template<
typename OtherScalar>
610 const OtherScalar & alpha)
616 ReturnType
res =
A *
exp6(alpha*dv);
623 #include "pinocchio/spatial/log.hxx"
625 #endif //#ifndef __pinocchio_spatial_explog_hpp__