pinocchio  3.3.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 
33  template<
34  typename LieGroup_t,
35  typename Scalar,
36  int Options,
37  template<typename, int> class JointCollectionTpl,
38  typename ConfigVectorType,
39  typename TangentVectorType,
40  typename ReturnType>
41  void integrate(
42  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
43  const Eigen::MatrixBase<ConfigVectorType> & q,
44  const Eigen::MatrixBase<TangentVectorType> & v,
45  const Eigen::MatrixBase<ReturnType> & qout);
46 
63  template<
64  typename Scalar,
65  int Options,
66  template<typename, int> class JointCollectionTpl,
67  typename ConfigVectorType,
68  typename TangentVectorType,
69  typename ReturnType>
70  void integrate(
71  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
72  const Eigen::MatrixBase<ConfigVectorType> & q,
73  const Eigen::MatrixBase<TangentVectorType> & v,
74  const Eigen::MatrixBase<ReturnType> & qout)
75  {
76  integrate<
77  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
78  ReturnType>(model, q.derived(), v.derived(), qout.derived());
79  }
80 
93  template<
94  typename LieGroup_t,
95  typename Scalar,
96  int Options,
97  template<typename, int> class JointCollectionTpl,
98  typename ConfigVectorIn1,
99  typename ConfigVectorIn2,
100  typename ReturnType>
101  void interpolate(
102  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
103  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
104  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
105  const Scalar & u,
106  const Eigen::MatrixBase<ReturnType> & qout);
107 
120  template<
121  typename Scalar,
122  int Options,
123  template<typename, int> class JointCollectionTpl,
124  typename ConfigVectorIn1,
125  typename ConfigVectorIn2,
126  typename ReturnType>
127  void interpolate(
128  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
129  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
130  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
131  const Scalar & u,
132  const Eigen::MatrixBase<ReturnType> & qout)
133  {
134  interpolate<
135  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
136  ReturnType>(
137  model, q0.derived(), q1.derived(), u, PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout));
138  }
139 
156  template<
157  typename LieGroup_t,
158  typename Scalar,
159  int Options,
160  template<typename, int> class JointCollectionTpl,
161  typename ConfigVectorIn1,
162  typename ConfigVectorIn2,
163  typename ReturnType>
164  void difference(
165  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
166  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
167  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
168  const Eigen::MatrixBase<ReturnType> & dvout);
169 
186  template<
187  typename Scalar,
188  int Options,
189  template<typename, int> class JointCollectionTpl,
190  typename ConfigVectorIn1,
191  typename ConfigVectorIn2,
192  typename ReturnType>
193  void difference(
194  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
195  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
196  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
197  const Eigen::MatrixBase<ReturnType> & dvout)
198  {
199  difference<
200  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
201  ReturnType>(model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(ReturnType, dvout));
202  }
203 
216  template<
217  typename LieGroup_t,
218  typename Scalar,
219  int Options,
220  template<typename, int> class JointCollectionTpl,
221  typename ConfigVectorIn1,
222  typename ConfigVectorIn2,
223  typename ReturnType>
224  void squaredDistance(
225  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
226  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
227  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
228  const Eigen::MatrixBase<ReturnType> & out);
229 
242  template<
243  typename Scalar,
244  int Options,
245  template<typename, int> class JointCollectionTpl,
246  typename ConfigVectorIn1,
247  typename ConfigVectorIn2,
248  typename ReturnType>
249  void squaredDistance(
250  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
251  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
252  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
253  const Eigen::MatrixBase<ReturnType> & out)
254  {
255  squaredDistance<
256  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
257  ReturnType>(model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(ReturnType, out));
258  }
259 
277  template<
278  typename LieGroup_t,
279  typename Scalar,
280  int Options,
281  template<typename, int> class JointCollectionTpl,
282  typename ConfigVectorIn1,
283  typename ConfigVectorIn2,
284  typename ReturnType>
285  void randomConfiguration(
286  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
287  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
288  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
289  const Eigen::MatrixBase<ReturnType> & qout);
290 
308  template<
309  typename Scalar,
310  int Options,
311  template<typename, int> class JointCollectionTpl,
312  typename ConfigVectorIn1,
313  typename ConfigVectorIn2,
314  typename ReturnType>
315  void randomConfiguration(
316  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
317  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
318  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
319  const Eigen::MatrixBase<ReturnType> & qout)
320  {
321  randomConfiguration<
322  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
323  ReturnType>(
324  model, lowerLimits.derived(), upperLimits.derived(),
325  PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout));
326  }
327 
338  template<
339  typename LieGroup_t,
340  typename Scalar,
341  int Options,
342  template<typename, int> class JointCollectionTpl,
343  typename ReturnType>
344  void neutral(
345  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
346  const Eigen::MatrixBase<ReturnType> & qout);
347 
358  template<
359  typename Scalar,
360  int Options,
361  template<typename, int> class JointCollectionTpl,
362  typename ReturnType>
363  void neutral(
364  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
365  const Eigen::MatrixBase<ReturnType> & qout)
366  {
367  neutral<LieGroupMap, Scalar, Options, JointCollectionTpl, ReturnType>(
368  model, PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout));
369  }
370 
395  template<
396  typename LieGroup_t,
397  typename Scalar,
398  int Options,
399  template<typename, int> class JointCollectionTpl,
400  typename ConfigVectorType,
401  typename TangentVectorType,
402  typename JacobianMatrixType>
403  void dIntegrate(
404  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
405  const Eigen::MatrixBase<ConfigVectorType> & q,
406  const Eigen::MatrixBase<TangentVectorType> & v,
407  const Eigen::MatrixBase<JacobianMatrixType> & J,
408  const ArgumentPosition arg,
409  const AssignmentOperatorType op = SETTO);
410 
435  template<
436  typename Scalar,
437  int Options,
438  template<typename, int> class JointCollectionTpl,
439  typename ConfigVectorType,
440  typename TangentVectorType,
441  typename JacobianMatrixType>
442  void dIntegrate(
443  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
444  const Eigen::MatrixBase<ConfigVectorType> & q,
445  const Eigen::MatrixBase<TangentVectorType> & v,
446  const Eigen::MatrixBase<JacobianMatrixType> & J,
447  const ArgumentPosition arg)
448  {
449  dIntegrate<
450  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
451  JacobianMatrixType>(
452  model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, J), arg,
453  SETTO);
454  }
455 
480  template<
481  typename Scalar,
482  int Options,
483  template<typename, int> class JointCollectionTpl,
484  typename ConfigVectorType,
485  typename TangentVectorType,
486  typename JacobianMatrixType>
487  void dIntegrate(
488  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
489  const Eigen::MatrixBase<ConfigVectorType> & q,
490  const Eigen::MatrixBase<TangentVectorType> & v,
491  const Eigen::MatrixBase<JacobianMatrixType> & J,
492  const ArgumentPosition arg,
493  const AssignmentOperatorType op)
494  {
495  dIntegrate<
496  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
497  JacobianMatrixType>(
498  model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, J), arg, op);
499  }
500 
526  template<
527  typename LieGroup_t,
528  typename Scalar,
529  int Options,
530  template<typename, int> class JointCollectionTpl,
531  typename ConfigVectorType,
532  typename TangentVectorType,
533  typename JacobianMatrixType1,
534  typename JacobianMatrixType2>
535  void dIntegrateTransport(
536  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
537  const Eigen::MatrixBase<ConfigVectorType> & q,
538  const Eigen::MatrixBase<TangentVectorType> & v,
539  const Eigen::MatrixBase<JacobianMatrixType1> & Jin,
540  const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
541  const ArgumentPosition arg);
542 
568  template<
569  typename Scalar,
570  int Options,
571  template<typename, int> class JointCollectionTpl,
572  typename ConfigVectorType,
573  typename TangentVectorType,
574  typename JacobianMatrixType1,
575  typename JacobianMatrixType2>
576  void dIntegrateTransport(
577  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
578  const Eigen::MatrixBase<ConfigVectorType> & q,
579  const Eigen::MatrixBase<TangentVectorType> & v,
580  const Eigen::MatrixBase<JacobianMatrixType1> & Jin,
581  const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
582  const ArgumentPosition arg)
583  {
584  dIntegrateTransport<
585  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
586  JacobianMatrixType1, JacobianMatrixType2>(
587  model, q.derived(), v.derived(), Jin.derived(),
588  PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType2, Jout), arg);
589  }
590 
615  template<
616  typename LieGroup_t,
617  typename Scalar,
618  int Options,
619  template<typename, int> class JointCollectionTpl,
620  typename ConfigVectorType,
621  typename TangentVectorType,
622  typename JacobianMatrixType>
623  void dIntegrateTransport(
624  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
625  const Eigen::MatrixBase<ConfigVectorType> & q,
626  const Eigen::MatrixBase<TangentVectorType> & v,
627  const Eigen::MatrixBase<JacobianMatrixType> & J,
628  const ArgumentPosition arg);
629 
654  template<
655  typename Scalar,
656  int Options,
657  template<typename, int> class JointCollectionTpl,
658  typename ConfigVectorType,
659  typename TangentVectorType,
660  typename JacobianMatrixType>
661  void dIntegrateTransport(
662  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
663  const Eigen::MatrixBase<ConfigVectorType> & q,
664  const Eigen::MatrixBase<TangentVectorType> & v,
665  const Eigen::MatrixBase<JacobianMatrixType> & J,
666  const ArgumentPosition arg)
667  {
668  dIntegrateTransport<
669  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
670  JacobianMatrixType>(
671  model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, J), arg);
672  }
673 
697  template<
698  typename LieGroup_t,
699  typename Scalar,
700  int Options,
701  template<typename, int> class JointCollectionTpl,
702  typename ConfigVector1,
703  typename ConfigVector2,
704  typename JacobianMatrix>
705  void dDifference(
706  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
707  const Eigen::MatrixBase<ConfigVector1> & q0,
708  const Eigen::MatrixBase<ConfigVector2> & q1,
709  const Eigen::MatrixBase<JacobianMatrix> & J,
710  const ArgumentPosition arg);
711 
735  template<
736  typename Scalar,
737  int Options,
738  template<typename, int> class JointCollectionTpl,
739  typename ConfigVector1,
740  typename ConfigVector2,
741  typename JacobianMatrix>
742  void dDifference(
743  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
744  const Eigen::MatrixBase<ConfigVector1> & q0,
745  const Eigen::MatrixBase<ConfigVector2> & q1,
746  const Eigen::MatrixBase<JacobianMatrix> & J,
747  const ArgumentPosition arg)
748  {
749  dDifference<
750  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVector1, ConfigVector2,
751  JacobianMatrix>(
752  model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, J), arg);
753  }
765  template<
766  typename LieGroup_t,
767  typename Scalar,
768  int Options,
769  template<typename, int> class JointCollectionTpl,
770  typename ConfigVectorIn1,
771  typename ConfigVectorIn2>
772  Scalar squaredDistanceSum(
773  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
774  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
775  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
776 
789  template<
790  typename Scalar,
791  int Options,
792  template<typename, int> class JointCollectionTpl,
793  typename ConfigVectorIn1,
794  typename ConfigVectorIn2>
795  Scalar squaredDistanceSum(
796  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
797  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
798  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
799  {
800  return squaredDistanceSum<
801  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
802  model, q0.derived(), q1.derived());
803  }
804 
817  template<
818  typename LieGroup_t,
819  typename Scalar,
820  int Options,
821  template<typename, int> class JointCollectionTpl,
822  typename ConfigVectorIn1,
823  typename ConfigVectorIn2>
824  Scalar distance(
825  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
826  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
827  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
828 
840  template<
841  typename Scalar,
842  int Options,
843  template<typename, int> class JointCollectionTpl,
844  typename ConfigVectorIn1,
845  typename ConfigVectorIn2>
846  Scalar distance(
847  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
848  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
849  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
850  {
851  return distance<
852  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
853  model, q0.derived(), q1.derived());
854  }
855 
864  template<
865  typename LieGroup_t,
866  typename Scalar,
867  int Options,
868  template<typename, int> class JointCollectionTpl,
869  typename ConfigVectorType>
870  void normalize(
871  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
872  const Eigen::MatrixBase<ConfigVectorType> & qout);
873 
882  template<
883  typename Scalar,
884  int Options,
885  template<typename, int> class JointCollectionTpl,
886  typename ConfigVectorType>
887  void normalize(
888  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
889  const Eigen::MatrixBase<ConfigVectorType> & qout)
890  {
891  normalize<LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType>(
892  model, PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, qout));
893  }
894 
906  template<
907  typename LieGroup_t,
908  typename Scalar,
909  int Options,
910  template<typename, int> class JointCollectionTpl,
911  typename ConfigVectorType>
912  bool isNormalized(
913  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
914  const Eigen::MatrixBase<ConfigVectorType> & q,
915  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision());
916 
928  template<
929  typename Scalar,
930  int Options,
931  template<typename, int> class JointCollectionTpl,
932  typename ConfigVectorType>
933  bool isNormalized(
934  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
935  const Eigen::MatrixBase<ConfigVectorType> & q,
936  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
937  {
938  return isNormalized<LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType>(
939  model, q, prec);
940  }
941 
958  template<
959  typename LieGroup_t,
960  typename Scalar,
961  int Options,
962  template<typename, int> class JointCollectionTpl,
963  typename ConfigVectorIn1,
964  typename ConfigVectorIn2>
965  bool isSameConfiguration(
966  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
967  const Eigen::MatrixBase<ConfigVectorIn1> & q1,
968  const Eigen::MatrixBase<ConfigVectorIn2> & q2,
969  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision());
970 
987  template<
988  typename Scalar,
989  int Options,
990  template<typename, int> class JointCollectionTpl,
991  typename ConfigVectorIn1,
992  typename ConfigVectorIn2>
993  bool isSameConfiguration(
994  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
995  const Eigen::MatrixBase<ConfigVectorIn1> & q1,
996  const Eigen::MatrixBase<ConfigVectorIn2> & q2,
997  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
998  {
999  return isSameConfiguration<
1000  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1001  model, q1.derived(), q2.derived(), prec);
1002  }
1003 
1016  template<
1017  typename LieGroup_t,
1018  typename Scalar,
1019  int Options,
1020  template<typename, int> class JointCollectionTpl,
1021  typename ConfigVector,
1022  typename JacobianMatrix>
1023  void integrateCoeffWiseJacobian(
1024  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1025  const Eigen::MatrixBase<ConfigVector> & q,
1026  const Eigen::MatrixBase<JacobianMatrix> & jacobian);
1027 
1040  template<
1041  typename Scalar,
1042  int Options,
1043  template<typename, int> class JointCollectionTpl,
1044  typename ConfigVector,
1045  typename JacobianMatrix>
1046  void integrateCoeffWiseJacobian(
1047  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1048  const Eigen::MatrixBase<ConfigVector> & q,
1049  const Eigen::MatrixBase<JacobianMatrix> & jacobian)
1050  {
1051  integrateCoeffWiseJacobian<
1052  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVector, JacobianMatrix>(
1053  model, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, jacobian));
1054  }
1055 
1057 
1060 
1074  template<
1075  typename LieGroup_t,
1076  typename Scalar,
1077  int Options,
1078  template<typename, int> class JointCollectionTpl,
1079  typename ConfigVectorType,
1080  typename TangentVectorType>
1081  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(
1082  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1083  const Eigen::MatrixBase<ConfigVectorType> & q,
1084  const Eigen::MatrixBase<TangentVectorType> & v);
1085 
1099  template<
1100  typename Scalar,
1101  int Options,
1102  template<typename, int> class JointCollectionTpl,
1103  typename ConfigVectorType,
1104  typename TangentVectorType>
1105  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(
1106  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1107  const Eigen::MatrixBase<ConfigVectorType> & q,
1108  const Eigen::MatrixBase<TangentVectorType> & v)
1109  {
1110  return integrate<
1111  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType>(
1112  model, q.derived(), v.derived());
1113  }
1114 
1128  template<
1129  typename LieGroup_t,
1130  typename Scalar,
1131  int Options,
1132  template<typename, int> class JointCollectionTpl,
1133  typename ConfigVectorIn1,
1134  typename ConfigVectorIn2>
1135  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) interpolate(
1136  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1137  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1138  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
1139  const Scalar & u);
1140 
1154  template<
1155  typename Scalar,
1156  int Options,
1157  template<typename, int> class JointCollectionTpl,
1158  typename ConfigVectorIn1,
1159  typename ConfigVectorIn2>
1160  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) interpolate(
1161  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1162  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1163  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
1164  const Scalar & u)
1165  {
1166  return interpolate<
1167  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1168  model, q0.derived(), q1.derived(), u);
1169  }
1170 
1183  template<
1184  typename LieGroup_t,
1185  typename Scalar,
1186  int Options,
1187  template<typename, int> class JointCollectionTpl,
1188  typename ConfigVectorIn1,
1189  typename ConfigVectorIn2>
1190  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) difference(
1191  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1192  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1193  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
1194 
1207  template<
1208  typename Scalar,
1209  int Options,
1210  template<typename, int> class JointCollectionTpl,
1211  typename ConfigVectorIn1,
1212  typename ConfigVectorIn2>
1213  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) difference(
1214  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1215  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1216  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
1217  {
1218  return difference<
1219  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1220  model, q0.derived(), q1.derived());
1221  }
1222 
1236  template<
1237  typename LieGroup_t,
1238  typename Scalar,
1239  int Options,
1240  template<typename, int> class JointCollectionTpl,
1241  typename ConfigVectorIn1,
1242  typename ConfigVectorIn2>
1243  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) squaredDistance(
1244  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1245  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1246  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
1247 
1261  template<
1262  typename Scalar,
1263  int Options,
1264  template<typename, int> class JointCollectionTpl,
1265  typename ConfigVectorIn1,
1266  typename ConfigVectorIn2>
1267  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) squaredDistance(
1268  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1269  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1270  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
1271  {
1272  return squaredDistance<
1273  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1274  model, q0.derived(), q1.derived());
1275  }
1276 
1295  template<
1296  typename LieGroup_t,
1297  typename Scalar,
1298  int Options,
1299  template<typename, int> class JointCollectionTpl,
1300  typename ConfigVectorIn1,
1301  typename ConfigVectorIn2>
1302  typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS(
1303  (typename ModelTpl<Scalar, Options, JointCollectionTpl>::ConfigVectorType))
1304  randomConfiguration(
1305  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1306  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
1307  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits);
1308 
1327  template<
1328  typename Scalar,
1329  int Options,
1330  template<typename, int> class JointCollectionTpl,
1331  typename ConfigVectorIn1,
1332  typename ConfigVectorIn2>
1333  typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS(
1334  (typename ModelTpl<Scalar, Options, JointCollectionTpl>::ConfigVectorType))
1335  randomConfiguration(
1336  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1337  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
1338  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits)
1339  {
1340  return randomConfiguration<
1341  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1342  model, lowerLimits.derived(), upperLimits.derived());
1343  }
1344 
1363  template<
1364  typename LieGroup_t,
1365  typename Scalar,
1366  int Options,
1367  template<typename, int> class JointCollectionTpl>
1368  typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS(
1369  (typename ModelTpl<Scalar, Options, JointCollectionTpl>::ConfigVectorType))
1370  randomConfiguration(const ModelTpl<Scalar, Options, JointCollectionTpl> & model);
1371 
1390  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
1391  typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS(
1392  (typename ModelTpl<Scalar, Options, JointCollectionTpl>::ConfigVectorType))
1393  randomConfiguration(const ModelTpl<Scalar, Options, JointCollectionTpl> & model)
1394  {
1395  return randomConfiguration<LieGroupMap, Scalar, Options, JointCollectionTpl>(model);
1396  }
1397 
1408  template<
1409  typename LieGroup_t,
1410  typename Scalar,
1411  int Options,
1412  template<typename, int> class JointCollectionTpl>
1413  Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options>
1414  neutral(const ModelTpl<Scalar, Options, JointCollectionTpl> & model);
1415 
1424  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
1425  Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options>
1426  neutral(const ModelTpl<Scalar, Options, JointCollectionTpl> & model)
1427  {
1428  return neutral<LieGroupMap, Scalar, Options, JointCollectionTpl>(model);
1429  }
1430 
1432 
1433 } // namespace pinocchio
1434 
1435 /* --- Details -------------------------------------------------------------------- */
1436 #include "pinocchio/algorithm/joint-configuration.hxx"
1437 
1438 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
1439  #include "pinocchio/algorithm/joint-configuration.txx"
1440 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
1441 
1442 #endif // ifndef __pinocchio_algorithm_joint_configuration_hpp__
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::ArgumentPosition
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: fwd.hpp:122
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:193
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:993
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.
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:487
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:249
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:933
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:795
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:315
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:1046
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:846
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:127
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 initial tangent space of the integrate operation,...
Definition: joint-configuration.hpp:576
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:887
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:363
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:70
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:742
pinocchio::ModelTpl::ConfigVectorType
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Definition: model.hpp:87