pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-configuration.hpp
1 //
2 // Copyright (c) 2016-2020 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_algorithm_joint_configuration_hpp__
6 #define __pinocchio_algorithm_joint_configuration_hpp__
7 
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/multibody/liegroup/liegroup.hpp"
10 
11 namespace pinocchio
12 {
13 
16 
31  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ReturnType>
32  void
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);
37 
52  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ReturnType>
53  void
54  integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
55  const Eigen::MatrixBase<ConfigVectorType> & q,
56  const Eigen::MatrixBase<TangentVectorType> & v,
57  const Eigen::MatrixBase<ReturnType> & qout)
58  {
59  integrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,ReturnType>(model, q.derived(), v.derived(), qout.derived());
60  }
61 
74  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
75  void
76  interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
77  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
78  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
79  const Scalar & u,
80  const Eigen::MatrixBase<ReturnType> & qout);
81 
94  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
95  void
96  interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
97  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
98  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
99  const Scalar & u,
100  const Eigen::MatrixBase<ReturnType> & qout)
101  {
102  interpolate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model, q0.derived(), q1.derived(), u, PINOCCHIO_EIGEN_CONST_CAST(ReturnType,qout));
103  }
104 
119  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
120  void
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);
125 
140  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
141  void
142  difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
143  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
144  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
145  const Eigen::MatrixBase<ReturnType> & dvout)
146  {
147  difference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model,q0.derived(),q1.derived(),PINOCCHIO_EIGEN_CONST_CAST(ReturnType,dvout));
148  }
149 
160  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
161  void
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);
166 
177  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
178  void
179  squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
180  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
181  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
182  const Eigen::MatrixBase<ReturnType> & out)
183  {
184  squaredDistance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model,q0.derived(),q1.derived(),PINOCCHIO_EIGEN_CONST_CAST(ReturnType,out));
185  }
186 
201  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
202  void
203  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
204  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
205  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
206  const Eigen::MatrixBase<ReturnType> & qout);
207 
222  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
223  void
224  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
225  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
226  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
227  const Eigen::MatrixBase<ReturnType> & qout)
228  {
229  randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model, lowerLimits.derived(), upperLimits.derived(), PINOCCHIO_EIGEN_CONST_CAST(ReturnType,qout));
230  }
231 
241  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ReturnType>
242  void
243  neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
244  const Eigen::MatrixBase<ReturnType> & qout);
245 
255  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ReturnType>
256  void
257  neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
258  const Eigen::MatrixBase<ReturnType> & qout)
259  {
260  neutral<LieGroupMap,Scalar,Options,JointCollectionTpl,ReturnType>(model,PINOCCHIO_EIGEN_CONST_CAST(ReturnType,qout));
261  }
262 
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,
286  const ArgumentPosition arg,
287  const AssignmentOperatorType op=SETTO);
288 
307  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
308  void dIntegrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
309  const Eigen::MatrixBase<ConfigVectorType> & q,
310  const Eigen::MatrixBase<TangentVectorType> & v,
311  const Eigen::MatrixBase<JacobianMatrixType> & J,
312  const ArgumentPosition arg)
313  {
314  dIntegrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg,SETTO);
315  }
316 
317 
336  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
337  void dIntegrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
338  const Eigen::MatrixBase<ConfigVectorType> & q,
339  const Eigen::MatrixBase<TangentVectorType> & v,
340  const Eigen::MatrixBase<JacobianMatrixType> & J,
341  const ArgumentPosition arg,
342  const AssignmentOperatorType op)
343  {
344  dIntegrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg,op);
345  }
346 
367  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType1, typename JacobianMatrixType2>
368  void dIntegrateTransport(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
369  const Eigen::MatrixBase<ConfigVectorType> & q,
370  const Eigen::MatrixBase<TangentVectorType> & v,
371  const Eigen::MatrixBase<JacobianMatrixType1> & Jin,
372  const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
373  const ArgumentPosition arg);
374 
395  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType1, typename JacobianMatrixType2>
396  void dIntegrateTransport(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
397  const Eigen::MatrixBase<ConfigVectorType> & q,
398  const Eigen::MatrixBase<TangentVectorType> & v,
399  const Eigen::MatrixBase<JacobianMatrixType1> & Jin,
400  const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
401  const ArgumentPosition arg)
402  {
403  dIntegrateTransport<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType1,JacobianMatrixType2>(model, q.derived(), v.derived(), Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType2,Jout),arg);
404  }
405 
425  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
426  void dIntegrateTransport(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
427  const Eigen::MatrixBase<ConfigVectorType> & q,
428  const Eigen::MatrixBase<TangentVectorType> & v,
429  const Eigen::MatrixBase<JacobianMatrixType> & J,
430  const ArgumentPosition arg);
431 
451  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
452  void dIntegrateTransport(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
453  const Eigen::MatrixBase<ConfigVectorType> & q,
454  const Eigen::MatrixBase<TangentVectorType> & v,
455  const Eigen::MatrixBase<JacobianMatrixType> & J,
456  const ArgumentPosition arg)
457  {
458  dIntegrateTransport<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg);
459  }
460 
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,
484  const ArgumentPosition arg);
485 
504  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector1, typename ConfigVector2, typename JacobianMatrix>
505  void dDifference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
506  const Eigen::MatrixBase<ConfigVector1> & q0,
507  const Eigen::MatrixBase<ConfigVector2> & q1,
508  const Eigen::MatrixBase<JacobianMatrix> & J,
509  const ArgumentPosition arg)
510  {
511  dDifference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVector1,ConfigVector2,JacobianMatrix>
512  (model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,J),arg);
513  }
525  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
526  Scalar squaredDistanceSum(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
527  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
528  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
529 
541  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
542  inline Scalar
543  squaredDistanceSum(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
544  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
545  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
546  {
547  return squaredDistanceSum<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived());
548  }
549 
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);
565 
577  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
578  inline Scalar
579  distance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
580  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
581  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
582  {
583  return distance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived());
584  }
585 
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);
597 
606  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
607  inline void normalize(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
608  const Eigen::MatrixBase<ConfigVectorType> & qout)
609  {
610  normalize<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType>(model,PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType,qout));
611  }
612 
626  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
627  inline bool
628  isSameConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
629  const Eigen::MatrixBase<ConfigVectorIn1> & q1,
630  const Eigen::MatrixBase<ConfigVectorIn2> & q2,
631  const Scalar & prec);
632 
646  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
647  inline bool
648  isSameConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
649  const Eigen::MatrixBase<ConfigVectorIn1> & q1,
650  const Eigen::MatrixBase<ConfigVectorIn2> & q2,
651  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
652  {
653  return isSameConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q1.derived(), q2.derived(), prec);
654  }
655 
667  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector, typename JacobianMatrix>
668  inline void
669  integrateCoeffWiseJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
670  const Eigen::MatrixBase<ConfigVector> & q,
671  const Eigen::MatrixBase<JacobianMatrix> & jacobian);
672 
684  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector, typename JacobianMatrix>
685  inline void
686  integrateCoeffWiseJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
687  const Eigen::MatrixBase<ConfigVector> & q,
688  const Eigen::MatrixBase<JacobianMatrix> & jacobian)
689  {
690  integrateCoeffWiseJacobian<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVector,JacobianMatrix>(model,q.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,jacobian));
691  }
692 
694 
697 
709  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
710  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType)
711  integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
712  const Eigen::MatrixBase<ConfigVectorType> & q,
713  const Eigen::MatrixBase<TangentVectorType> & v);
714 
726  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
727  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType)
728  integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
729  const Eigen::MatrixBase<ConfigVectorType> & q,
730  const Eigen::MatrixBase<TangentVectorType> & v)
731  {
732  return integrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType>(model, q.derived(), v.derived());
733  }
734 
747  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
748  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
749  interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
750  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
751  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
752  const Scalar & u);
753 
766  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
767  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
768  interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
769  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
770  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
771  const Scalar & u)
772  {
773  return interpolate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived(), u);
774  }
775 
787  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
788  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
789  difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
790  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
791  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
792 
804  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
805  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
806  difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
807  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
808  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
809  {
810  return difference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model,q0.derived(),q1.derived());
811  }
812 
824  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
825  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
826  squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
827  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
828  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
829 
841  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
842  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
843  squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
844  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
845  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
846  {
847  return squaredDistance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model,q0.derived(),q1.derived());
848  }
849 
865  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
866  typename PINOCCHIO_EIGEN_PLAIN_TYPE((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
867  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
868  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
869  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits);
870 
886  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
887  typename PINOCCHIO_EIGEN_PLAIN_TYPE((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
888  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
889  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
890  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits)
891  {
892  return randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, lowerLimits.derived(), upperLimits.derived());
893  }
894 
909  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
910  typename PINOCCHIO_EIGEN_PLAIN_TYPE((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
911  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model);
912 
927  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
928  typename PINOCCHIO_EIGEN_PLAIN_TYPE((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
929  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model)
930  {
931  return randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl>(model);
932  }
933 
943  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
944  inline Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options>
945  neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model);
946 
954  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
955  inline Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options>
956  neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model)
957  {
958  return neutral<LieGroupMap,Scalar,Options,JointCollectionTpl>(model);
959  }
960 
962 
963 } // namespace pinocchio
964 
965 /* --- Details -------------------------------------------------------------------- */
966 #include "pinocchio/algorithm/joint-configuration.hxx"
967 
968 #endif // ifndef __pinocchio_algorithm_joint_configuration_hpp__
pinocchio::distance
Scalar distance(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1)
Distance between two configuration vectors, namely .
Definition: joint-configuration.hpp:579
pinocchio::integrateCoeffWiseJacobian
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.
Definition: joint-configuration.hpp:686
pinocchio::squaredDistanceSum
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.
Definition: joint-configuration.hpp:543
pinocchio::integrate
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...
Definition: joint-configuration.hpp:54
pinocchio::dDifference
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...
Definition: joint-configuration.hpp:505
pinocchio::difference
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...
Definition: joint-configuration.hpp:142
Eigen
Definition: casadi.hpp:47
pinocchio::ModelTpl::ConfigVectorType
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Definition: model.hpp:66
pinocchio::randomConfiguration
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.
Definition: joint-configuration.hpp:224
pinocchio::isSameConfiguration
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.
Definition: joint-configuration.hpp:648
pinocchio::dIntegrateTransport
void dIntegrateTransport(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< JacobianMatrixType1 > &Jin, const Eigen::MatrixBase< JacobianMatrixType2 > &Jout, const ArgumentPosition arg)
Transport a matrix from the terminal to the originate tangent space of the integrate operation...
Definition: joint-configuration.hpp:396
pinocchio::dIntegrate
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, const AssignmentOperatorType op=SETTO)
Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the...
Definition: joint-configuration.hpp:337
pinocchio::interpolate
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.
Definition: joint-configuration.hpp:96
pinocchio::ArgumentPosition
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: fwd.hpp:50
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24
pinocchio::neutral
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
Definition: joint-configuration.hpp:257
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:607
pinocchio::squaredDistance
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.
Definition: joint-configuration.hpp:179