5 #ifndef __pinocchio_algorithm_joint_configuration_hpp__
6 #define __pinocchio_algorithm_joint_configuration_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>
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>
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>
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)
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,
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,
344 dIntegrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(
model,
q.derived(),
v.derived(),
PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,
J),arg,op);
368 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType,
typename JacobianMatrixType1,
typename JacobianMatrixType2>
370 const Eigen::MatrixBase<ConfigVectorType> &
q,
371 const Eigen::MatrixBase<TangentVectorType> &
v,
372 const Eigen::MatrixBase<JacobianMatrixType1> & Jin,
373 const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
397 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType,
typename JacobianMatrixType1,
typename JacobianMatrixType2>
399 const Eigen::MatrixBase<ConfigVectorType> &
q,
400 const Eigen::MatrixBase<TangentVectorType> &
v,
401 const Eigen::MatrixBase<JacobianMatrixType1> & Jin,
402 const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
405 dIntegrateTransport<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType1,JacobianMatrixType2>(
model,
q.derived(),
v.derived(), Jin.derived(),
PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType2,Jout),arg);
427 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType,
typename JacobianMatrixType>
429 const Eigen::MatrixBase<ConfigVectorType> &
q,
430 const Eigen::MatrixBase<TangentVectorType> &
v,
431 const Eigen::MatrixBase<JacobianMatrixType> &
J,
453 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType,
typename JacobianMatrixType>
455 const Eigen::MatrixBase<ConfigVectorType> &
q,
456 const Eigen::MatrixBase<TangentVectorType> &
v,
457 const Eigen::MatrixBase<JacobianMatrixType> &
J,
460 dIntegrateTransport<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(
model,
q.derived(),
v.derived(),
PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,
J),arg);
481 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVector1,
typename ConfigVector2,
typename JacobianMatrix>
483 const Eigen::MatrixBase<ConfigVector1> &
q0,
484 const Eigen::MatrixBase<ConfigVector2> &
q1,
485 const Eigen::MatrixBase<JacobianMatrix> &
J,
506 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVector1,
typename ConfigVector2,
typename JacobianMatrix>
508 const Eigen::MatrixBase<ConfigVector1> &
q0,
509 const Eigen::MatrixBase<ConfigVector2> &
q1,
510 const Eigen::MatrixBase<JacobianMatrix> &
J,
513 dDifference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVector1,ConfigVector2,JacobianMatrix>
527 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
529 const Eigen::MatrixBase<ConfigVectorIn1> &
q0,
530 const Eigen::MatrixBase<ConfigVectorIn2> &
q1);
543 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
546 const Eigen::MatrixBase<ConfigVectorIn1> &
q0,
547 const Eigen::MatrixBase<ConfigVectorIn2> &
q1)
549 return squaredDistanceSum<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(
model,
q0.derived(),
q1.derived());
563 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
565 const Eigen::MatrixBase<ConfigVectorIn1> &
q0,
566 const Eigen::MatrixBase<ConfigVectorIn2> &
q1);
579 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
582 const Eigen::MatrixBase<ConfigVectorIn1> &
q0,
583 const Eigen::MatrixBase<ConfigVectorIn2> &
q1)
585 return distance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(
model,
q0.derived(),
q1.derived());
596 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType>
597 inline void normalize(
const ModelTpl<Scalar,Options,JointCollectionTpl> &
model,
598 const Eigen::MatrixBase<ConfigVectorType> & qout);
608 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType>
610 const Eigen::MatrixBase<ConfigVectorType> & qout)
625 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType>
626 inline bool isNormalized(
const ModelTpl<Scalar,Options,JointCollectionTpl> &
model,
627 const Eigen::MatrixBase<ConfigVectorType> &
q,
628 const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision());
640 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType>
642 const Eigen::MatrixBase<ConfigVectorType> &
q,
643 const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision())
645 return isNormalized<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType>(
model,
q,prec);
661 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
664 const Eigen::MatrixBase<ConfigVectorIn1> &
q1,
665 const Eigen::MatrixBase<ConfigVectorIn2> & q2,
666 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision());
681 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
684 const Eigen::MatrixBase<ConfigVectorIn1> &
q1,
685 const Eigen::MatrixBase<ConfigVectorIn2> & q2,
686 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
688 return isSameConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(
model,
q1.derived(),
q2.derived(), prec);
702 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVector,
typename JacobianMatrix>
705 const Eigen::MatrixBase<ConfigVector> &
q,
706 const Eigen::MatrixBase<JacobianMatrix> & jacobian);
719 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVector,
typename JacobianMatrix>
722 const Eigen::MatrixBase<ConfigVector> &
q,
723 const Eigen::MatrixBase<JacobianMatrix> & jacobian)
725 integrateCoeffWiseJacobian<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVector,JacobianMatrix>(
model,
q.derived(),
PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,jacobian));
744 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType>
747 const
Eigen::MatrixBase<ConfigVectorType> &
q,
748 const
Eigen::MatrixBase<TangentVectorType> &
v);
761 template<typename
Scalar,
int Options, template<typename,
int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
764 const
Eigen::MatrixBase<ConfigVectorType> &
q,
765 const
Eigen::MatrixBase<TangentVectorType> &
v)
767 return integrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType>(
model,
q.derived(),
v.derived());
782 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
785 const Eigen::MatrixBase<ConfigVectorIn1> &
q0,
786 const Eigen::MatrixBase<ConfigVectorIn2> &
q1,
801 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
804 const Eigen::MatrixBase<ConfigVectorIn1> &
q0,
805 const Eigen::MatrixBase<ConfigVectorIn2> &
q1,
808 return interpolate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(
model,
q0.derived(),
q1.derived(),
u);
822 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
825 const Eigen::MatrixBase<ConfigVectorIn1> &
q0,
826 const Eigen::MatrixBase<ConfigVectorIn2> &
q1);
839 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
842 const Eigen::MatrixBase<ConfigVectorIn1> &
q0,
843 const Eigen::MatrixBase<ConfigVectorIn2> &
q1)
845 return difference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(
model,
q0.derived(),
q1.derived());
859 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
862 const Eigen::MatrixBase<ConfigVectorIn1> &
q0,
863 const Eigen::MatrixBase<ConfigVectorIn2> &
q1);
876 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
879 const Eigen::MatrixBase<ConfigVectorIn1> &
q0,
880 const Eigen::MatrixBase<ConfigVectorIn2> &
q1)
882 return squaredDistance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(
model,
q0.derived(),
q1.derived());
900 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
921 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
924 const Eigen::MatrixBase<ConfigVectorIn1> &
lowerLimits,
925 const Eigen::MatrixBase<ConfigVectorIn2> &
upperLimits)
927 return randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(
model,
lowerLimits.derived(),
upperLimits.derived());
944 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
962 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
966 return randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl>(
model);
978 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
979 inline Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options>
980 neutral(
const ModelTpl<Scalar,Options,JointCollectionTpl> &
model);
989 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
990 inline Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options>
993 return neutral<LieGroupMap,Scalar,Options,JointCollectionTpl>(
model);
1001 #include "pinocchio/algorithm/joint-configuration.hxx"
1003 #endif // ifndef __pinocchio_algorithm_joint_configuration_hpp__