pinocchio  2.1.3
joint-configuration.hpp
1 //
2 // Copyright (c) 2016-2018 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_joint_configuration_hpp__
6 #define __pinocchio_joint_configuration_hpp__
7 
8 #include "pinocchio/multibody/fwd.hpp"
9 #include "pinocchio/multibody/liegroup/liegroup.hpp"
10 
11 namespace pinocchio
12 {
13 
16 
28  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ReturnType>
29  void
30  integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
31  const Eigen::MatrixBase<ConfigVectorType> & q,
32  const Eigen::MatrixBase<TangentVectorType> & v,
33  const Eigen::MatrixBase<ReturnType> & qout);
34 
46  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ReturnType>
47  void
48  integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
49  const Eigen::MatrixBase<ConfigVectorType> & q,
50  const Eigen::MatrixBase<TangentVectorType> & v,
51  const Eigen::MatrixBase<ReturnType> & qout)
52  {
53  integrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,ReturnType>(model, q.derived(), v.derived(), qout.derived());
54  }
55 
65  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
66  void
67  interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
68  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
69  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
70  const Scalar & u,
71  const Eigen::MatrixBase<ReturnType> & qout);
72 
82  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
83  void
84  interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
85  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
86  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
87  const Scalar & u,
88  const Eigen::MatrixBase<ReturnType> & qout)
89  {
90  interpolate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model, q0, q1, u, qout);
91  }
92 
104  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
105  void
106  difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
107  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
108  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
109  const Eigen::MatrixBase<ReturnType> & dvout);
110 
122  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
123  void
124  difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
125  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
126  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
127  const Eigen::MatrixBase<ReturnType> & dvout)
128  {
129  difference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model,q0.derived(),q1.derived(),dvout.derived());
130  }
131 
140  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
141  void
142  squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
143  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
144  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
145  const Eigen::MatrixBase<ReturnType> & out);
146 
155  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
156  void
157  squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
158  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
159  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
160  const Eigen::MatrixBase<ReturnType> & out)
161  {
162  squaredDistance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model,q0.derived(),q1.derived(),out.derived());
163  }
164 
177  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
178  void
179  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
180  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
181  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
182  const Eigen::MatrixBase<ReturnType> & qout);
183 
196  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
197  void
198  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
199  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
200  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
201  const Eigen::MatrixBase<ReturnType> & qout)
202  {
203  randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model, lowerLimits.derived(), upperLimits.derived(), qout.derived());
204  }
205 
212  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ReturnType>
213  void
214  neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, const Eigen::MatrixBase<ReturnType> & qout);
215 
222  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ReturnType>
223  void
224  neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, const Eigen::MatrixBase<ReturnType> & qout)
225  {
226  neutral<LieGroupMap,Scalar,Options,JointCollectionTpl,ReturnType>(model,qout.derived());
227  }
228 
246  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
247  void dIntegrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
248  const Eigen::MatrixBase<ConfigVectorType> & q,
249  const Eigen::MatrixBase<TangentVectorType> & v,
250  const Eigen::MatrixBase<JacobianMatrixType> & J,
251  const ArgumentPosition arg);
252 
270  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
271  void dIntegrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
272  const Eigen::MatrixBase<ConfigVectorType> & q,
273  const Eigen::MatrixBase<TangentVectorType> & v,
274  const Eigen::MatrixBase<JacobianMatrixType> & J,
275  const ArgumentPosition arg)
276  {
277  dIntegrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), J.derived(),arg);
278  }
279 
289  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
290  Scalar squaredDistanceSum(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
291  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
292  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
293 
303  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
304  inline Scalar
305  squaredDistanceSum(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
306  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
307  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
308  {
309  return squaredDistanceSum<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived());
310  }
311 
321  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
322  Scalar distance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
323  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
324  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
325 
335  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
336  inline Scalar
337  distance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
338  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
339  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
340  {
341  return distance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived());
342  }
343 
351  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
352  inline void normalize(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
353  const Eigen::MatrixBase<ConfigVectorType> & qout);
354 
362  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
363  inline void normalize(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
364  const Eigen::MatrixBase<ConfigVectorType> & qout)
365  {
366  normalize<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType>(model,qout.derived());
367  }
368 
380  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
381  inline bool
382  isSameConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
383  const Eigen::MatrixBase<ConfigVectorIn1> & q1,
384  const Eigen::MatrixBase<ConfigVectorIn2> & q2,
385  const Scalar & prec);
386 
398  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
399  inline bool
400  isSameConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
401  const Eigen::MatrixBase<ConfigVectorIn1> & q1,
402  const Eigen::MatrixBase<ConfigVectorIn2> & q2,
403  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
404  {
405  return isSameConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q1.derived(), q2.derived(), prec);
406  }
407 
418  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector, typename JacobianMatrix>
419  inline void
420  integrateCoeffWiseJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
421  const Eigen::MatrixBase<ConfigVector> & q,
422  const Eigen::MatrixBase<JacobianMatrix> & jacobian);
423 
434  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector, typename JacobianMatrix>
435  inline void
436  integrateCoeffWiseJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
437  const Eigen::MatrixBase<ConfigVector> & q,
438  const Eigen::MatrixBase<JacobianMatrix> & jacobian)
439  {
440  integrateCoeffWiseJacobian<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVector,JacobianMatrix>(model,q,jacobian);
441  }
442 
444 
447 
457  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
458  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType)
459  integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
460  const Eigen::MatrixBase<ConfigVectorType> & q,
461  const Eigen::MatrixBase<TangentVectorType> & v);
462 
472  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
473  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType)
474  integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
475  const Eigen::MatrixBase<ConfigVectorType> & q,
476  const Eigen::MatrixBase<TangentVectorType> & v)
477  {
478  return integrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType>(model, q.derived(), v.derived());
479  }
480 
491  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
492  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
493  interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
494  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
495  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
496  const Scalar & u);
497 
508  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
509  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
510  interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
511  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
512  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
513  const Scalar & u)
514  {
515  return interpolate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0, q1, u);
516  }
517 
527  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
528  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
529  difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
530  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
531  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
532 
542  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
543  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
544  difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
545  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
546  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
547  {
548  return difference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model,q0.derived(),q1.derived());
549  }
550 
560  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
561  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
562  squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
563  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
564  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
565 
575  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
576  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
577  squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
578  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
579  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
580  {
581  return squaredDistance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model,q0.derived(),q1.derived());
582  }
583 
597  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
598  typename PINOCCHIO_EIGEN_PLAIN_TYPE((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
599  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
600  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
601  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits);
602 
616  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
617  typename PINOCCHIO_EIGEN_PLAIN_TYPE((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
618  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
619  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
620  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits)
621  {
622  return randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, lowerLimits.derived(), upperLimits.derived());
623  }
624 
637  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
638  typename PINOCCHIO_EIGEN_PLAIN_TYPE((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
639  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model);
640 
653  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
654  typename PINOCCHIO_EIGEN_PLAIN_TYPE((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
655  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model)
656  {
657  return randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl>(model);
658  }
659 
667  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
668  inline Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options>
669  neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model);
670 
678  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
679  inline Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options>
680  neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model)
681  {
682  return neutral<LieGroupMap,Scalar,Options,JointCollectionTpl>(model);
683  }
684 
686 
687 } // namespace pinocchio
688 
689 /* --- Details -------------------------------------------------------------------- */
690 #include "pinocchio/algorithm/joint-configuration.hxx"
691 
692 #endif // ifndef __pinocchio_joint_configuration_hpp__
693 
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.
Definition: joint-configuration.hpp:337
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:436
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:305
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:48
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)
Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the...
Definition: joint-configuration.hpp:271
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:124
Eigen
Source from #include <cppad/example/cppad_eigen.hpp>
Definition: cppad.hpp:24
pinocchio::ModelTpl::ConfigVectorType
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Definition: multibody/model.hpp:65
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:198
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.
Definition: joint-configuration.hpp:400
pinocchio::jacobian
PINOCCHIO_DEPRECATED void jacobian(const Model &model, Data &data, const Eigen::VectorXd &q, const Model::JointIndex jointId, Data::Matrix6x &J)
Computes the Jacobian of a specific joint frame expressed in the local frame of the joint and store t...
Definition: jacobian.hpp:201
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:84
pinocchio::ArgumentPosition
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: spatial/fwd.hpp:39
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:224
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:363
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:157