pinocchio  2.4.0-dirty
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-configuration.hpp
1 //
2 // Copyright (c) 2016-2019 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/model.hpp"
10 
11 #include "pinocchio/multibody/liegroup/liegroup.hpp"
12 
13 namespace pinocchio
14 {
15 
18 
33  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ReturnType>
34  void
35  integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
36  const Eigen::MatrixBase<ConfigVectorType> & q,
37  const Eigen::MatrixBase<TangentVectorType> & v,
38  const Eigen::MatrixBase<ReturnType> & qout);
39 
54  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ReturnType>
55  void
57  const Eigen::MatrixBase<ConfigVectorType> & q,
58  const Eigen::MatrixBase<TangentVectorType> & v,
59  const Eigen::MatrixBase<ReturnType> & qout)
60  {
61  integrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,ReturnType>(model, q.derived(), v.derived(), qout.derived());
62  }
63 
76  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
77  void
79  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
80  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
81  const Scalar & u,
82  const Eigen::MatrixBase<ReturnType> & qout);
83 
96  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
97  void
99  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
100  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
101  const Scalar & u,
102  const Eigen::MatrixBase<ReturnType> & qout)
103  {
104  interpolate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model, q0.derived(), q1.derived(), u, PINOCCHIO_EIGEN_CONST_CAST(ReturnType,qout));
105  }
106 
121  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
122  void
124  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
125  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
126  const Eigen::MatrixBase<ReturnType> & dvout);
127 
142  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
143  void
145  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
146  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
147  const Eigen::MatrixBase<ReturnType> & dvout)
148  {
149  difference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model,q0.derived(),q1.derived(),PINOCCHIO_EIGEN_CONST_CAST(ReturnType,dvout));
150  }
151 
162  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
163  void
165  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
166  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
167  const Eigen::MatrixBase<ReturnType> & out);
168 
179  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
180  void
182  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
183  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
184  const Eigen::MatrixBase<ReturnType> & out)
185  {
186  squaredDistance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model,q0.derived(),q1.derived(),PINOCCHIO_EIGEN_CONST_CAST(ReturnType,out));
187  }
188 
203  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
204  void
206  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
207  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
208  const Eigen::MatrixBase<ReturnType> & qout);
209 
224  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2, typename ReturnType>
225  void
227  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
228  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
229  const Eigen::MatrixBase<ReturnType> & qout)
230  {
231  randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2,ReturnType>(model, lowerLimits.derived(), upperLimits.derived(), PINOCCHIO_EIGEN_CONST_CAST(ReturnType,qout));
232  }
233 
243  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ReturnType>
244  void
246  const Eigen::MatrixBase<ReturnType> & qout);
247 
257  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ReturnType>
258  void
260  const Eigen::MatrixBase<ReturnType> & qout)
261  {
262  neutral<LieGroupMap,Scalar,Options,JointCollectionTpl,ReturnType>(model,PINOCCHIO_EIGEN_CONST_CAST(ReturnType,qout));
263  }
264 
283  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
285  const Eigen::MatrixBase<ConfigVectorType> & q,
286  const Eigen::MatrixBase<TangentVectorType> & v,
287  const Eigen::MatrixBase<JacobianMatrixType> & J,
288  const ArgumentPosition arg);
289 
308  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
310  const Eigen::MatrixBase<ConfigVectorType> & q,
311  const Eigen::MatrixBase<TangentVectorType> & v,
312  const Eigen::MatrixBase<JacobianMatrixType> & J,
313  const ArgumentPosition arg)
314  {
315  dIntegrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg);
316  }
317 
336  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector1, typename ConfigVector2, typename JacobianMatrix>
338  const Eigen::MatrixBase<ConfigVector1> & q0,
339  const Eigen::MatrixBase<ConfigVector2> & q1,
340  const Eigen::MatrixBase<JacobianMatrix> & J,
341  const ArgumentPosition arg);
342 
361  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector1, typename ConfigVector2, typename JacobianMatrix>
363  const Eigen::MatrixBase<ConfigVector1> & q0,
364  const Eigen::MatrixBase<ConfigVector2> & q1,
365  const Eigen::MatrixBase<JacobianMatrix> & J,
366  const ArgumentPosition arg)
367  {
368  dDifference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVector1,ConfigVector2,JacobianMatrix>
369  (model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,J),arg);
370  }
382  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
384  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
385  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
386 
398  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
399  inline Scalar
401  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
402  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
403  {
404  return squaredDistanceSum<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived());
405  }
406 
418  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
420  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
421  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
422 
434  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
435  inline Scalar
437  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
438  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
439  {
440  return distance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived());
441  }
442 
451  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
452  inline void normalize(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
453  const Eigen::MatrixBase<ConfigVectorType> & qout);
454 
463  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
465  const Eigen::MatrixBase<ConfigVectorType> & qout)
466  {
467  normalize<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType>(model,PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType,qout));
468  }
469 
483  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
484  inline bool
486  const Eigen::MatrixBase<ConfigVectorIn1> & q1,
487  const Eigen::MatrixBase<ConfigVectorIn2> & q2,
488  const Scalar & prec);
489 
503  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
504  inline bool
506  const Eigen::MatrixBase<ConfigVectorIn1> & q1,
507  const Eigen::MatrixBase<ConfigVectorIn2> & q2,
508  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
509  {
510  return isSameConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q1.derived(), q2.derived(), prec);
511  }
512 
524  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector, typename JacobianMatrix>
525  inline void
527  const Eigen::MatrixBase<ConfigVector> & q,
528  const Eigen::MatrixBase<JacobianMatrix> & jacobian);
529 
541  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector, typename JacobianMatrix>
542  inline void
544  const Eigen::MatrixBase<ConfigVector> & q,
545  const Eigen::MatrixBase<JacobianMatrix> & jacobian)
546  {
547  integrateCoeffWiseJacobian<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVector,JacobianMatrix>(model,q.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,jacobian));
548  }
549 
551 
554 
566  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
567  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType)
569  const Eigen::MatrixBase<ConfigVectorType> & q,
570  const Eigen::MatrixBase<TangentVectorType> & v);
571 
583  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
584  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType)
585  integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
586  const Eigen::MatrixBase<ConfigVectorType> & q,
587  const Eigen::MatrixBase<TangentVectorType> & v)
588  {
589  return integrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType>(model, q.derived(), v.derived());
590  }
591 
604  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
605  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
607  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
608  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
609  const Scalar & u);
610 
623  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
624  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
625  interpolate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
626  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
627  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
628  const Scalar & u)
629  {
630  return interpolate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived(), u);
631  }
632 
644  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
645  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
647  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
648  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
649 
661  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
662  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
663  difference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
664  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
665  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
666  {
667  return difference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model,q0.derived(),q1.derived());
668  }
669 
681  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
682  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
684  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
685  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
686 
698  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
699  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
700  squaredDistance(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
701  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
702  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
703  {
704  return squaredDistance<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model,q0.derived(),q1.derived());
705  }
706 
722  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
723  typename PINOCCHIO_EIGEN_PLAIN_TYPE((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
725  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
726  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits);
727 
743  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
744  typename PINOCCHIO_EIGEN_PLAIN_TYPE((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
745  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
746  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
747  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits)
748  {
749  return randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorIn1,ConfigVectorIn2>(model, lowerLimits.derived(), upperLimits.derived());
750  }
751 
766  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
767  typename PINOCCHIO_EIGEN_PLAIN_TYPE((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
769 
784  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
785  typename PINOCCHIO_EIGEN_PLAIN_TYPE((typename ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType))
786  randomConfiguration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model)
787  {
788  return randomConfiguration<LieGroupMap,Scalar,Options,JointCollectionTpl>(model);
789  }
790 
800  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
801  inline Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options>
803 
811  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
812  inline Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options>
814  {
815  return neutral<LieGroupMap,Scalar,Options,JointCollectionTpl>(model);
816  }
817 
819 
820 } // namespace pinocchio
821 
822 /* --- Details -------------------------------------------------------------------- */
823 #include "pinocchio/algorithm/joint-configuration.hxx"
824 
825 #endif // ifndef __pinocchio_joint_configuration_hpp__
826 
Scalar distance(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1)
Distance between two configuration vectors, namely .
void integrateCoeffWiseJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVector > &q, const Eigen::MatrixBase< JacobianMatrix > &jacobian)
Return the Jacobian of the integrate function for the components of the config vector.
Scalar squaredDistanceSum(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1)
Overall squared distance between two configuration vectors.
void integrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< ReturnType > &qout)
Integrate a configuration vector for the specified model for a tangent vector during one unit time...
void dIntegrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< JacobianMatrixType > &J, const ArgumentPosition arg)
Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the...
void dDifference(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVector1 > &q0, const Eigen::MatrixBase< ConfigVector2 > &q1, const Eigen::MatrixBase< JacobianMatrix > &J, const ArgumentPosition arg)
Computes the Jacobian of a small variation of the configuration vector into the tangent space at iden...
void difference(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1, const Eigen::MatrixBase< ReturnType > &dvout)
Compute the tangent vector that must be integrated during one unit time to go from q0 to q1...
Definition: casadi.hpp:40
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Definition: model.hpp:66
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
bool isSameConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q1, const Eigen::MatrixBase< ConfigVectorIn2 > &q2, const Scalar &prec)
Return true if the given configurations are equivalents, within the given precision.
void interpolate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1, const Scalar &u, const Eigen::MatrixBase< ReturnType > &qout)
Interpolate two configurations for a given model.
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: fwd.hpp:50
Main pinocchio namespace.
Definition: treeview.dox:24
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
void squaredDistance(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1, const Eigen::MatrixBase< ReturnType > &out)
Squared distance between two configuration vectors.