pinocchio  2.6.0
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
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
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
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
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
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
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>
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>
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>
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>
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>
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
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
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>
608  const Eigen::MatrixBase<ConfigVectorType> & qout)
609  {
610  normalize<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType>(model,PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType,qout));
611  }
612 
623  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
624  inline bool isNormalized(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
625  const Eigen::MatrixBase<ConfigVectorType> & q,
626  const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision());
627 
638  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
640  const Eigen::MatrixBase<ConfigVectorType> & q,
641  const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision())
642  {
643  return isNormalized<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType>(model,q,prec);
644  }
645 
659  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
660  inline bool
661  isSameConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
662  const Eigen::MatrixBase<ConfigVectorIn1> & q1,
663  const Eigen::MatrixBase<ConfigVectorIn2> & q2,
664  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision());
665 
679  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
680  inline bool
682  const Eigen::MatrixBase<ConfigVectorIn1> & q1,
683  const Eigen::MatrixBase<ConfigVectorIn2> & q2,
684  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
685  {
686  return isSameConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q1.derived(), q2.derived(), prec);
687  }
688 
700  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector, typename JacobianMatrix>
701  inline void
702  integrateCoeffWiseJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
703  const Eigen::MatrixBase<ConfigVector> & q,
704  const Eigen::MatrixBase<JacobianMatrix> & jacobian);
705 
717  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector, typename JacobianMatrix>
718  inline void
720  const Eigen::MatrixBase<ConfigVector> & q,
721  const Eigen::MatrixBase<JacobianMatrix> & jacobian)
722  {
723  integrateCoeffWiseJacobian<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVector,JacobianMatrix>(model,q.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,jacobian));
724  }
725 
727 
730 
742  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
743  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType)
744  integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
745  const Eigen::MatrixBase<ConfigVectorType> & q,
746  const Eigen::MatrixBase<TangentVectorType> & v);
747 
759  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
760  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType)
761  integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
762  const Eigen::MatrixBase<ConfigVectorType> & q,
763  const Eigen::MatrixBase<TangentVectorType> & v)
764  {
765  return integrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType>(model, q.derived(), v.derived());
766  }
767 
780  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
781  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
782  interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
783  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
784  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
785  const Scalar & u);
786 
799  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
800  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
801  interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
802  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
803  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
804  const Scalar & u)
805  {
806  return interpolate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived(), u);
807  }
808 
820  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
821  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
822  difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
823  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
824  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
825 
837  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
838  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
839  difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
840  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
841  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
842  {
843  return difference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model,q0.derived(),q1.derived());
844  }
845 
857  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
858  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
859  squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
860  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
861  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
862 
874  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
875  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
876  squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
877  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
878  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
879  {
880  return squaredDistance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model,q0.derived(),q1.derived());
881  }
882 
898  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
900  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
901  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
902  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits);
903 
919  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
921  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
922  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
923  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits)
924  {
925  return randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, lowerLimits.derived(), upperLimits.derived());
926  }
927 
942  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
944  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model);
945 
960  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
962  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model)
963  {
964  return randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl>(model);
965  }
966 
976  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
977  inline Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options>
978  neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model);
979 
987  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
988  inline Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options>
990  {
991  return neutral<LieGroupMap,Scalar,Options,JointCollectionTpl>(model);
992  }
993 
995 
996 } // namespace pinocchio
997 
998 /* --- Details -------------------------------------------------------------------- */
999 #include "pinocchio/algorithm/joint-configuration.hxx"
1000 
1001 #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::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:396
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:639
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::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:505
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:719
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:681
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
Main pinocchio namespace.
Definition: treeview.dox:24
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.