pinocchio  2.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-configuration.hpp
1 //
2 // Copyright (c) 2016-2021 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 
368  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType1, typename JacobianMatrixType2>
369  void dIntegrateTransport(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
370  const Eigen::MatrixBase<ConfigVectorType> & q,
371  const Eigen::MatrixBase<TangentVectorType> & v,
372  const Eigen::MatrixBase<JacobianMatrixType1> & Jin,
373  const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
374  const ArgumentPosition arg);
375 
397  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType1, typename JacobianMatrixType2>
398  void dIntegrateTransport(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
399  const Eigen::MatrixBase<ConfigVectorType> & q,
400  const Eigen::MatrixBase<TangentVectorType> & v,
401  const Eigen::MatrixBase<JacobianMatrixType1> & Jin,
402  const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
403  const ArgumentPosition arg)
404  {
405  dIntegrateTransport<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType1,JacobianMatrixType2>(model, q.derived(), v.derived(), Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType2,Jout),arg);
406  }
407 
427  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
428  void dIntegrateTransport(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
429  const Eigen::MatrixBase<ConfigVectorType> & q,
430  const Eigen::MatrixBase<TangentVectorType> & v,
431  const Eigen::MatrixBase<JacobianMatrixType> & J,
432  const ArgumentPosition arg);
433 
453  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
454  void dIntegrateTransport(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
455  const Eigen::MatrixBase<ConfigVectorType> & q,
456  const Eigen::MatrixBase<TangentVectorType> & v,
457  const Eigen::MatrixBase<JacobianMatrixType> & J,
458  const ArgumentPosition arg)
459  {
460  dIntegrateTransport<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg);
461  }
462 
481  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector1, typename ConfigVector2, typename JacobianMatrix>
482  void dDifference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
483  const Eigen::MatrixBase<ConfigVector1> & q0,
484  const Eigen::MatrixBase<ConfigVector2> & q1,
485  const Eigen::MatrixBase<JacobianMatrix> & J,
486  const ArgumentPosition arg);
487 
506  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector1, typename ConfigVector2, typename JacobianMatrix>
507  void dDifference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
508  const Eigen::MatrixBase<ConfigVector1> & q0,
509  const Eigen::MatrixBase<ConfigVector2> & q1,
510  const Eigen::MatrixBase<JacobianMatrix> & J,
511  const ArgumentPosition arg)
512  {
513  dDifference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVector1,ConfigVector2,JacobianMatrix>
514  (model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,J),arg);
515  }
527  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
528  Scalar squaredDistanceSum(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
529  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
530  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
531 
543  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
544  inline Scalar
545  squaredDistanceSum(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
546  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
547  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
548  {
549  return squaredDistanceSum<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived());
550  }
551 
563  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
564  Scalar distance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
565  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
566  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
567 
579  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
580  inline Scalar
581  distance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
582  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
583  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
584  {
585  return distance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived());
586  }
587 
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);
599 
608  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
609  inline void normalize(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
610  const Eigen::MatrixBase<ConfigVectorType> & qout)
611  {
612  normalize<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType>(model,PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType,qout));
613  }
614 
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());
629 
640  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
641  inline bool isNormalized(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
642  const Eigen::MatrixBase<ConfigVectorType> & q,
643  const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision())
644  {
645  return isNormalized<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType>(model,q,prec);
646  }
647 
661  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
662  inline bool
663  isSameConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
664  const Eigen::MatrixBase<ConfigVectorIn1> & q1,
665  const Eigen::MatrixBase<ConfigVectorIn2> & q2,
666  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision());
667 
681  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
682  inline bool
683  isSameConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
684  const Eigen::MatrixBase<ConfigVectorIn1> & q1,
685  const Eigen::MatrixBase<ConfigVectorIn2> & q2,
686  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
687  {
688  return isSameConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q1.derived(), q2.derived(), prec);
689  }
690 
702  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector, typename JacobianMatrix>
703  inline void
704  integrateCoeffWiseJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
705  const Eigen::MatrixBase<ConfigVector> & q,
706  const Eigen::MatrixBase<JacobianMatrix> & jacobian);
707 
719  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector, typename JacobianMatrix>
720  inline void
721  integrateCoeffWiseJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
722  const Eigen::MatrixBase<ConfigVector> & q,
723  const Eigen::MatrixBase<JacobianMatrix> & jacobian)
724  {
725  integrateCoeffWiseJacobian<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVector,JacobianMatrix>(model,q.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,jacobian));
726  }
727 
729 
732 
744  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
745  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType)
746  integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
747  const Eigen::MatrixBase<ConfigVectorType> & q,
748  const Eigen::MatrixBase<TangentVectorType> & v);
749 
761  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
762  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType)
763  integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
764  const Eigen::MatrixBase<ConfigVectorType> & q,
765  const Eigen::MatrixBase<TangentVectorType> & v)
766  {
767  return integrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType>(model, q.derived(), v.derived());
768  }
769 
782  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
783  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
784  interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
785  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
786  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
787  const Scalar & u);
788 
801  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
802  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
803  interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
804  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
805  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
806  const Scalar & u)
807  {
808  return interpolate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived(), u);
809  }
810 
822  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
823  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
824  difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
825  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
826  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
827 
839  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
840  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
841  difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
842  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
843  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
844  {
845  return difference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model,q0.derived(),q1.derived());
846  }
847 
859  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
860  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
861  squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
862  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
863  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
864 
876  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
877  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
878  squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
879  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
880  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
881  {
882  return squaredDistance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model,q0.derived(),q1.derived());
883  }
884 
900  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
901  typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
902  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
903  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
904  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits);
905 
921  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
922  typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
923  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
924  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
925  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits)
926  {
927  return randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, lowerLimits.derived(), upperLimits.derived());
928  }
929 
944  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
945  typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
946  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model);
947 
962  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
963  typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
964  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model)
965  {
966  return randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl>(model);
967  }
968 
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);
981 
989  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
990  inline Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options>
991  neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model)
992  {
993  return neutral<LieGroupMap,Scalar,Options,JointCollectionTpl>(model);
994  }
995 
997 
998 } // namespace pinocchio
999 
1000 /* --- Details -------------------------------------------------------------------- */
1001 #include "pinocchio/algorithm/joint-configuration.hxx"
1002 
1003 #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:581
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::ModelTpl< Scalar, Options, JointCollectionTpl >::ConfigVectorType
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Definition: model.hpp:66
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::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
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:398
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::isNormalized
bool isNormalized(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
Check whether a configuration vector is normalized within the given precision provided by prec.
Definition: joint-configuration.hpp:641
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:545
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::ArgumentPosition
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: fwd.hpp:59
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
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:507
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::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:721
pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl >
pinocchio::isSameConfiguration
bool isSameConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q1, const Eigen::MatrixBase< ConfigVectorIn2 > &q2, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
Return true if the given configurations are equivalents, within the given precision.
Definition: joint-configuration.hpp:683
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:609
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS
PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS((typename ModelTpl< Scalar, Options, JointCollectionTpl >::ConfigVectorType)) randomConfiguration(const ModelTpl< Scalar
Generate a configuration vector uniformly sampled among given limits.