5 #ifndef __pinocchio_joint_configuration_hpp__ 6 #define __pinocchio_joint_configuration_hpp__ 8 #include "pinocchio/multibody/fwd.hpp" 9 #include "pinocchio/multibody/model.hpp" 11 #include "pinocchio/multibody/liegroup/liegroup.hpp" 33 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType,
typename ReturnType>
35 integrate(
const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
36 const Eigen::MatrixBase<ConfigVectorType> & q,
37 const Eigen::MatrixBase<TangentVectorType> & v,
38 const Eigen::MatrixBase<ReturnType> & qout);
54 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType,
typename ReturnType>
57 const Eigen::MatrixBase<ConfigVectorType> & q,
58 const Eigen::MatrixBase<TangentVectorType> & v,
59 const Eigen::MatrixBase<ReturnType> & qout)
61 integrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,ReturnType>(model, q.derived(), v.derived(), qout.derived());
76 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2,
typename ReturnType>
79 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
80 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
82 const Eigen::MatrixBase<ReturnType> & qout);
96 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2,
typename ReturnType>
99 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
100 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
102 const Eigen::MatrixBase<ReturnType> & qout)
104 interpolate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model, q0.derived(), q1.derived(), u, PINOCCHIO_EIGEN_CONST_CAST(ReturnType,qout));
121 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2,
typename ReturnType>
124 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
125 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
126 const Eigen::MatrixBase<ReturnType> & dvout);
142 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2,
typename ReturnType>
145 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
146 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
147 const Eigen::MatrixBase<ReturnType> & dvout)
149 difference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model,q0.derived(),q1.derived(),PINOCCHIO_EIGEN_CONST_CAST(ReturnType,dvout));
162 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2,
typename ReturnType>
165 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
166 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
167 const Eigen::MatrixBase<ReturnType> & out);
179 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2,
typename ReturnType>
182 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
183 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
184 const Eigen::MatrixBase<ReturnType> & out)
186 squaredDistance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model,q0.derived(),q1.derived(),PINOCCHIO_EIGEN_CONST_CAST(ReturnType,out));
203 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2,
typename ReturnType>
206 const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
207 const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
208 const Eigen::MatrixBase<ReturnType> & qout);
224 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2,
typename ReturnType>
227 const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
228 const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
229 const Eigen::MatrixBase<ReturnType> & qout)
231 randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model, lowerLimits.derived(), upperLimits.derived(), PINOCCHIO_EIGEN_CONST_CAST(ReturnType,qout));
243 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ReturnType>
246 const Eigen::MatrixBase<ReturnType> & qout);
257 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ReturnType>
260 const Eigen::MatrixBase<ReturnType> & qout)
262 neutral<LieGroupMap,Scalar,Options,JointCollectionTpl,ReturnType>(model,PINOCCHIO_EIGEN_CONST_CAST(ReturnType,qout));
283 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType,
typename JacobianMatrixType>
285 const Eigen::MatrixBase<ConfigVectorType> & q,
286 const Eigen::MatrixBase<TangentVectorType> & v,
287 const Eigen::MatrixBase<JacobianMatrixType> & J,
308 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType,
typename JacobianMatrixType>
310 const Eigen::MatrixBase<ConfigVectorType> & q,
311 const Eigen::MatrixBase<TangentVectorType> & v,
312 const Eigen::MatrixBase<JacobianMatrixType> & J,
315 dIntegrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg);
336 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVector1,
typename ConfigVector2,
typename JacobianMatrix>
338 const Eigen::MatrixBase<ConfigVector1> & q0,
339 const Eigen::MatrixBase<ConfigVector2> & q1,
340 const Eigen::MatrixBase<JacobianMatrix> & J,
361 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVector1,
typename ConfigVector2,
typename JacobianMatrix>
363 const Eigen::MatrixBase<ConfigVector1> & q0,
364 const Eigen::MatrixBase<ConfigVector2> & q1,
365 const Eigen::MatrixBase<JacobianMatrix> & J,
368 dDifference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVector1,ConfigVector2,JacobianMatrix>
369 (model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,J),arg);
382 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
384 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
385 const Eigen::MatrixBase<ConfigVectorIn2> & q1);
398 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
401 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
402 const Eigen::MatrixBase<ConfigVectorIn2> & q1)
404 return squaredDistanceSum<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived());
418 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
420 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
421 const Eigen::MatrixBase<ConfigVectorIn2> & q1);
434 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
437 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
438 const Eigen::MatrixBase<ConfigVectorIn2> & q1)
440 return distance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived());
451 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType>
453 const Eigen::MatrixBase<ConfigVectorType> & qout);
463 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType>
465 const Eigen::MatrixBase<ConfigVectorType> & qout)
467 normalize<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType>(model,PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType,qout));
483 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
486 const Eigen::MatrixBase<ConfigVectorIn1> & q1,
487 const Eigen::MatrixBase<ConfigVectorIn2> & q2,
488 const Scalar & prec);
503 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
506 const Eigen::MatrixBase<ConfigVectorIn1> & q1,
507 const Eigen::MatrixBase<ConfigVectorIn2> & q2,
508 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
510 return isSameConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q1.derived(), q2.derived(), prec);
524 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVector,
typename JacobianMatrix>
527 const Eigen::MatrixBase<ConfigVector> & q,
528 const Eigen::MatrixBase<JacobianMatrix> & jacobian);
541 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVector,
typename JacobianMatrix>
544 const Eigen::MatrixBase<ConfigVector> & q,
545 const Eigen::MatrixBase<JacobianMatrix> & jacobian)
547 integrateCoeffWiseJacobian<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVector,JacobianMatrix>(model,q.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,jacobian));
566 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType>
567 inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType)
569 const Eigen::MatrixBase<ConfigVectorType> & q,
570 const Eigen::MatrixBase<TangentVectorType> & v);
583 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType>
584 inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType)
585 integrate(const
ModelTpl<Scalar,Options,JointCollectionTpl> & model,
586 const
Eigen::MatrixBase<ConfigVectorType> & q,
587 const
Eigen::MatrixBase<TangentVectorType> & v)
589 return integrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType>(model, q.derived(), v.derived());
604 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
605 inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
607 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
608 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
623 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
624 inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
625 interpolate(const
ModelTpl<Scalar,Options,JointCollectionTpl> & model,
626 const
Eigen::MatrixBase<ConfigVectorIn1> & q0,
627 const
Eigen::MatrixBase<ConfigVectorIn2> & q1,
630 return interpolate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived(), u);
644 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
645 inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
647 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
648 const Eigen::MatrixBase<ConfigVectorIn2> & q1);
661 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
662 inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
664 const
Eigen::MatrixBase<ConfigVectorIn1> & q0,
665 const
Eigen::MatrixBase<ConfigVectorIn2> & q1)
667 return difference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model,q0.derived(),q1.derived());
681 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
682 inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
684 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
685 const Eigen::MatrixBase<ConfigVectorIn2> & q1);
698 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
699 inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
701 const
Eigen::MatrixBase<ConfigVectorIn1> & q0,
702 const
Eigen::MatrixBase<ConfigVectorIn2> & q1)
704 return squaredDistance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model,q0.derived(),q1.derived());
722 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
725 const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
726 const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits);
743 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorIn1,
typename ConfigVectorIn2>
746 const
Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
747 const
Eigen::MatrixBase<ConfigVectorIn2> & upperLimits)
749 return randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, lowerLimits.derived(), upperLimits.derived());
766 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
784 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
788 return randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl>(model);
800 template<
typename LieGroup_t,
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
801 inline Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options>
811 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
812 inline Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options>
815 return neutral<LieGroupMap,Scalar,Options,JointCollectionTpl>(model);
823 #include "pinocchio/algorithm/joint-configuration.hxx" 825 #endif // ifndef __pinocchio_joint_configuration_hpp__ Scalar distance(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1)
Distance between two configuration vectors, namely .
void integrateCoeffWiseJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVector > &q, const Eigen::MatrixBase< JacobianMatrix > &jacobian)
Return the Jacobian of the integrate function for the components of the config vector.
Scalar squaredDistanceSum(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1)
Overall squared distance between two configuration vectors.
void integrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< ReturnType > &qout)
Integrate a configuration vector for the specified model for a tangent vector during one unit time...
void dIntegrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< JacobianMatrixType > &J, const ArgumentPosition arg)
Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the...
void dDifference(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVector1 > &q0, const Eigen::MatrixBase< ConfigVector2 > &q1, const Eigen::MatrixBase< JacobianMatrix > &J, const ArgumentPosition arg)
Computes the Jacobian of a small variation of the configuration vector into the tangent space at iden...
void difference(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1, const Eigen::MatrixBase< ReturnType > &dvout)
Compute the tangent vector that must be integrated during one unit time to go from q0 to q1...
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
bool isSameConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q1, const Eigen::MatrixBase< ConfigVectorIn2 > &q2, const Scalar &prec)
Return true if the given configurations are equivalents, within the given precision.
void interpolate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1, const Scalar &u, const Eigen::MatrixBase< ReturnType > &qout)
Interpolate two configurations for a given model.
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Main pinocchio namespace.
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
void squaredDistance(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1, const Eigen::MatrixBase< ReturnType > &out)
Squared distance between two configuration vectors.