5 #ifndef __pinocchio_algorithm_joint_configuration_hpp__
6 #define __pinocchio_algorithm_joint_configuration_hpp__
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/multibody/liegroup/liegroup.hpp"
31 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType,
typename ReturnType>
33 integrate(
const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
34 const Eigen::MatrixBase<ConfigVectorType> & q,
35 const Eigen::MatrixBase<TangentVectorType> & v,
36 const Eigen::MatrixBase<ReturnType> & qout);
52 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType,
typename ReturnType>
55 const Eigen::MatrixBase<ConfigVectorType> & q,
56 const Eigen::MatrixBase<TangentVectorType> & v,
57 const Eigen::MatrixBase<ReturnType> & qout)
59 integrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,ReturnType>(model, q.derived(), v.derived(), qout.derived());
74 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2,
typename ReturnType>
76 interpolate(
const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
77 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
78 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
80 const Eigen::MatrixBase<ReturnType> & qout);
94 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2,
typename ReturnType>
97 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
98 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
100 const Eigen::MatrixBase<ReturnType> & qout)
102 interpolate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model, q0.derived(), q1.derived(), u, PINOCCHIO_EIGEN_CONST_CAST(ReturnType,qout));
119 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2,
typename ReturnType>
121 difference(
const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
122 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
123 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
124 const Eigen::MatrixBase<ReturnType> & dvout);
140 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2,
typename ReturnType>
143 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
144 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
145 const Eigen::MatrixBase<ReturnType> & dvout)
147 difference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model,q0.derived(),q1.derived(),PINOCCHIO_EIGEN_CONST_CAST(ReturnType,dvout));
160 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2,
typename ReturnType>
162 squaredDistance(
const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
163 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
164 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
165 const Eigen::MatrixBase<ReturnType> & out);
177 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2,
typename ReturnType>
180 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
181 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
182 const Eigen::MatrixBase<ReturnType> & out)
184 squaredDistance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model,q0.derived(),q1.derived(),PINOCCHIO_EIGEN_CONST_CAST(ReturnType,out));
201 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2,
typename ReturnType>
204 const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
205 const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
206 const Eigen::MatrixBase<ReturnType> & qout);
222 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2,
typename ReturnType>
225 const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
226 const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
227 const Eigen::MatrixBase<ReturnType> & qout)
229 randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model, lowerLimits.derived(), upperLimits.derived(), PINOCCHIO_EIGEN_CONST_CAST(ReturnType,qout));
241 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ReturnType>
243 neutral(
const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
244 const Eigen::MatrixBase<ReturnType> & qout);
255 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ReturnType>
258 const Eigen::MatrixBase<ReturnType> & qout)
260 neutral<LieGroupMap,Scalar,Options,JointCollectionTpl,ReturnType>(model,PINOCCHIO_EIGEN_CONST_CAST(ReturnType,qout));
281 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType,
typename JacobianMatrixType>
282 void dIntegrate(
const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
283 const Eigen::MatrixBase<ConfigVectorType> & q,
284 const Eigen::MatrixBase<TangentVectorType> & v,
285 const Eigen::MatrixBase<JacobianMatrixType> & J,
287 const AssignmentOperatorType op=SETTO);
307 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType,
typename JacobianMatrixType>
309 const Eigen::MatrixBase<ConfigVectorType> & q,
310 const Eigen::MatrixBase<TangentVectorType> & v,
311 const Eigen::MatrixBase<JacobianMatrixType> & J,
314 dIntegrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg,SETTO);
336 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType,
typename JacobianMatrixType>
338 const Eigen::MatrixBase<ConfigVectorType> & q,
339 const Eigen::MatrixBase<TangentVectorType> & v,
340 const Eigen::MatrixBase<JacobianMatrixType> & J,
342 const AssignmentOperatorType op)
344 dIntegrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg,op);
367 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType,
typename JacobianMatrixType1,
typename JacobianMatrixType2>
369 const Eigen::MatrixBase<ConfigVectorType> & q,
370 const Eigen::MatrixBase<TangentVectorType> & v,
371 const Eigen::MatrixBase<JacobianMatrixType1> & Jin,
372 const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
395 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType,
typename JacobianMatrixType1,
typename JacobianMatrixType2>
397 const Eigen::MatrixBase<ConfigVectorType> & q,
398 const Eigen::MatrixBase<TangentVectorType> & v,
399 const Eigen::MatrixBase<JacobianMatrixType1> & Jin,
400 const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
403 dIntegrateTransport<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType1,JacobianMatrixType2>(model, q.derived(), v.derived(), Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType2,Jout),arg);
425 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType,
typename JacobianMatrixType>
427 const Eigen::MatrixBase<ConfigVectorType> & q,
428 const Eigen::MatrixBase<TangentVectorType> & v,
429 const Eigen::MatrixBase<JacobianMatrixType> & J,
451 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType,
typename JacobianMatrixType>
453 const Eigen::MatrixBase<ConfigVectorType> & q,
454 const Eigen::MatrixBase<TangentVectorType> & v,
455 const Eigen::MatrixBase<JacobianMatrixType> & J,
458 dIntegrateTransport<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg);
479 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVector1,
typename ConfigVector2,
typename JacobianMatrix>
480 void dDifference(
const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
481 const Eigen::MatrixBase<ConfigVector1> & q0,
482 const Eigen::MatrixBase<ConfigVector2> & q1,
483 const Eigen::MatrixBase<JacobianMatrix> & J,
504 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVector1,
typename ConfigVector2,
typename JacobianMatrix>
506 const Eigen::MatrixBase<ConfigVector1> & q0,
507 const Eigen::MatrixBase<ConfigVector2> & q1,
508 const Eigen::MatrixBase<JacobianMatrix> & J,
511 dDifference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVector1,ConfigVector2,JacobianMatrix>
512 (model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,J),arg);
525 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
527 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
528 const Eigen::MatrixBase<ConfigVectorIn2> & q1);
541 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
544 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
545 const Eigen::MatrixBase<ConfigVectorIn2> & q1)
547 return squaredDistanceSum<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived());
561 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
562 Scalar
distance(
const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
563 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
564 const Eigen::MatrixBase<ConfigVectorIn2> & q1);
577 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
580 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
581 const Eigen::MatrixBase<ConfigVectorIn2> & q1)
583 return distance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived());
594 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType>
595 inline void normalize(
const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
596 const Eigen::MatrixBase<ConfigVectorType> & qout);
606 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType>
608 const Eigen::MatrixBase<ConfigVectorType> & qout)
610 normalize<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType>(model,PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType,qout));
623 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType>
624 inline bool isNormalized(
const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
625 const Eigen::MatrixBase<ConfigVectorType> & q,
626 const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision());
638 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType>
640 const Eigen::MatrixBase<ConfigVectorType> & q,
641 const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision())
643 return isNormalized<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType>(model,q,prec);
659 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
662 const Eigen::MatrixBase<ConfigVectorIn1> & q1,
663 const Eigen::MatrixBase<ConfigVectorIn2> & q2,
664 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision());
679 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
682 const Eigen::MatrixBase<ConfigVectorIn1> & q1,
683 const Eigen::MatrixBase<ConfigVectorIn2> & q2,
684 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
686 return isSameConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q1.derived(), q2.derived(), prec);
700 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVector,
typename JacobianMatrix>
703 const Eigen::MatrixBase<ConfigVector> & q,
704 const Eigen::MatrixBase<JacobianMatrix> & jacobian);
717 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVector,
typename JacobianMatrix>
720 const Eigen::MatrixBase<ConfigVector> & q,
721 const Eigen::MatrixBase<JacobianMatrix> & jacobian)
723 integrateCoeffWiseJacobian<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVector,JacobianMatrix>(model,q.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,jacobian));
742 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType>
743 inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType)
744 integrate(
const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
745 const Eigen::MatrixBase<ConfigVectorType> & q,
746 const Eigen::MatrixBase<TangentVectorType> & v);
759 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType>
760 inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType)
762 const Eigen::MatrixBase<ConfigVectorType> & q,
763 const Eigen::MatrixBase<TangentVectorType> & v)
765 return integrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType>(model, q.derived(), v.derived());
780 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
781 inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
782 interpolate(
const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
783 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
784 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
799 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
800 inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
802 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
803 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
806 return interpolate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived(), u);
820 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
821 inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
822 difference(
const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
823 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
824 const Eigen::MatrixBase<ConfigVectorIn2> & q1);
837 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
838 inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
840 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
841 const Eigen::MatrixBase<ConfigVectorIn2> & q1)
843 return difference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model,q0.derived(),q1.derived());
857 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
858 inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
859 squaredDistance(
const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
860 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
861 const Eigen::MatrixBase<ConfigVectorIn2> & q1);
874 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
875 inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
877 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
878 const Eigen::MatrixBase<ConfigVectorIn2> & q1)
880 return squaredDistance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model,q0.derived(),q1.derived());
898 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
901 const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
902 const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits);
919 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
922 const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
923 const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits)
925 return randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, lowerLimits.derived(), upperLimits.derived());
942 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
960 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
964 return randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl>(model);
976 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
977 inline Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options>
978 neutral(
const ModelTpl<Scalar,Options,JointCollectionTpl> & model);
987 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
988 inline Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options>
991 return neutral<LieGroupMap,Scalar,Options,JointCollectionTpl>(model);
999 #include "pinocchio/algorithm/joint-configuration.hxx"
1001 #endif // ifndef __pinocchio_algorithm_joint_configuration_hpp__