joint-configuration.hpp
Go to the documentation of this file.
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>
38  class JointCollectionTpl,
39  typename ConfigVectorType,
40  typename TangentVectorType,
41  typename ReturnType>
42  void integrate(
43  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
44  const Eigen::MatrixBase<ConfigVectorType> & q,
45  const Eigen::MatrixBase<TangentVectorType> & v,
46  const Eigen::MatrixBase<ReturnType> & qout);
47 
64  template<
65  typename Scalar,
66  int Options,
67  template<typename, int>
68  class JointCollectionTpl,
69  typename ConfigVectorType,
70  typename TangentVectorType,
71  typename ReturnType>
72  void integrate(
73  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
74  const Eigen::MatrixBase<ConfigVectorType> & q,
75  const Eigen::MatrixBase<TangentVectorType> & v,
76  const Eigen::MatrixBase<ReturnType> & qout)
77  {
78  integrate<
79  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
80  ReturnType>(model, q.derived(), v.derived(), qout.derived());
81  }
82 
95  template<
96  typename LieGroup_t,
97  typename Scalar,
98  int Options,
99  template<typename, int>
100  class JointCollectionTpl,
101  typename ConfigVectorIn1,
102  typename ConfigVectorIn2,
103  typename ReturnType>
104  void interpolate(
105  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
106  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
107  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
108  const Scalar & u,
109  const Eigen::MatrixBase<ReturnType> & qout);
110 
123  template<
124  typename Scalar,
125  int Options,
126  template<typename, int>
127  class JointCollectionTpl,
128  typename ConfigVectorIn1,
129  typename ConfigVectorIn2,
130  typename ReturnType>
131  void interpolate(
132  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
133  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
134  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
135  const Scalar & u,
136  const Eigen::MatrixBase<ReturnType> & qout)
137  {
138  interpolate<
139  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
140  ReturnType>(
141  model, q0.derived(), q1.derived(), u, PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout));
142  }
143 
160  template<
161  typename LieGroup_t,
162  typename Scalar,
163  int Options,
164  template<typename, int>
165  class JointCollectionTpl,
166  typename ConfigVectorIn1,
167  typename ConfigVectorIn2,
168  typename ReturnType>
169  void difference(
170  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
171  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
172  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
173  const Eigen::MatrixBase<ReturnType> & dvout);
174 
191  template<
192  typename Scalar,
193  int Options,
194  template<typename, int>
195  class JointCollectionTpl,
196  typename ConfigVectorIn1,
197  typename ConfigVectorIn2,
198  typename ReturnType>
199  void difference(
200  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
201  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
202  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
203  const Eigen::MatrixBase<ReturnType> & dvout)
204  {
205  difference<
206  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
207  ReturnType>(model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(ReturnType, dvout));
208  }
209 
222  template<
223  typename LieGroup_t,
224  typename Scalar,
225  int Options,
226  template<typename, int>
227  class JointCollectionTpl,
228  typename ConfigVectorIn1,
229  typename ConfigVectorIn2,
230  typename ReturnType>
231  void squaredDistance(
232  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
233  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
234  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
235  const Eigen::MatrixBase<ReturnType> & out);
236 
249  template<
250  typename Scalar,
251  int Options,
252  template<typename, int>
253  class JointCollectionTpl,
254  typename ConfigVectorIn1,
255  typename ConfigVectorIn2,
256  typename ReturnType>
257  void squaredDistance(
258  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
259  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
260  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
261  const Eigen::MatrixBase<ReturnType> & out)
262  {
263  squaredDistance<
264  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
265  ReturnType>(model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(ReturnType, out));
266  }
267 
285  template<
286  typename LieGroup_t,
287  typename Scalar,
288  int Options,
289  template<typename, int>
290  class JointCollectionTpl,
291  typename ConfigVectorIn1,
292  typename ConfigVectorIn2,
293  typename ReturnType>
294  void randomConfiguration(
295  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
296  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
297  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
298  const Eigen::MatrixBase<ReturnType> & qout);
299 
317  template<
318  typename Scalar,
319  int Options,
320  template<typename, int>
321  class JointCollectionTpl,
322  typename ConfigVectorIn1,
323  typename ConfigVectorIn2,
324  typename ReturnType>
325  void randomConfiguration(
326  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
327  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
328  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
329  const Eigen::MatrixBase<ReturnType> & qout)
330  {
331  randomConfiguration<
332  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
333  ReturnType>(
334  model, lowerLimits.derived(), upperLimits.derived(),
335  PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout));
336  }
337 
348  template<
349  typename LieGroup_t,
350  typename Scalar,
351  int Options,
352  template<typename, int>
353  class JointCollectionTpl,
354  typename ReturnType>
355  void neutral(
356  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
357  const Eigen::MatrixBase<ReturnType> & qout);
358 
369  template<
370  typename Scalar,
371  int Options,
372  template<typename, int>
373  class JointCollectionTpl,
374  typename ReturnType>
375  void neutral(
376  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
377  const Eigen::MatrixBase<ReturnType> & qout)
378  {
379  neutral<LieGroupMap, Scalar, Options, JointCollectionTpl, ReturnType>(
380  model, PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout));
381  }
382 
407  template<
408  typename LieGroup_t,
409  typename Scalar,
410  int Options,
411  template<typename, int>
412  class JointCollectionTpl,
413  typename ConfigVectorType,
414  typename TangentVectorType,
415  typename JacobianMatrixType>
416  void dIntegrate(
417  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
418  const Eigen::MatrixBase<ConfigVectorType> & q,
419  const Eigen::MatrixBase<TangentVectorType> & v,
420  const Eigen::MatrixBase<JacobianMatrixType> & J,
421  const ArgumentPosition arg,
422  const AssignmentOperatorType op = SETTO);
423 
448  template<
449  typename Scalar,
450  int Options,
451  template<typename, int>
452  class JointCollectionTpl,
453  typename ConfigVectorType,
454  typename TangentVectorType,
455  typename JacobianMatrixType>
456  void dIntegrate(
457  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
458  const Eigen::MatrixBase<ConfigVectorType> & q,
459  const Eigen::MatrixBase<TangentVectorType> & v,
460  const Eigen::MatrixBase<JacobianMatrixType> & J,
461  const ArgumentPosition arg)
462  {
463  dIntegrate<
464  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
465  JacobianMatrixType>(
466  model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, J), arg,
467  SETTO);
468  }
469 
494  template<
495  typename Scalar,
496  int Options,
497  template<typename, int>
498  class JointCollectionTpl,
499  typename ConfigVectorType,
500  typename TangentVectorType,
501  typename JacobianMatrixType>
502  void dIntegrate(
503  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
504  const Eigen::MatrixBase<ConfigVectorType> & q,
505  const Eigen::MatrixBase<TangentVectorType> & v,
506  const Eigen::MatrixBase<JacobianMatrixType> & J,
507  const ArgumentPosition arg,
508  const AssignmentOperatorType op)
509  {
510  dIntegrate<
511  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
512  JacobianMatrixType>(
513  model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, J), arg, op);
514  }
515 
541  template<
542  typename LieGroup_t,
543  typename Scalar,
544  int Options,
545  template<typename, int>
546  class JointCollectionTpl,
547  typename ConfigVectorType,
548  typename TangentVectorType,
549  typename JacobianMatrixType1,
550  typename JacobianMatrixType2>
551  void dIntegrateTransport(
552  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
553  const Eigen::MatrixBase<ConfigVectorType> & q,
554  const Eigen::MatrixBase<TangentVectorType> & v,
555  const Eigen::MatrixBase<JacobianMatrixType1> & Jin,
556  const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
557  const ArgumentPosition arg);
558 
584  template<
585  typename Scalar,
586  int Options,
587  template<typename, int>
588  class JointCollectionTpl,
589  typename ConfigVectorType,
590  typename TangentVectorType,
591  typename JacobianMatrixType1,
592  typename JacobianMatrixType2>
593  void dIntegrateTransport(
594  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
595  const Eigen::MatrixBase<ConfigVectorType> & q,
596  const Eigen::MatrixBase<TangentVectorType> & v,
597  const Eigen::MatrixBase<JacobianMatrixType1> & Jin,
598  const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
599  const ArgumentPosition arg)
600  {
601  dIntegrateTransport<
602  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
603  JacobianMatrixType1, JacobianMatrixType2>(
604  model, q.derived(), v.derived(), Jin.derived(),
605  PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType2, Jout), arg);
606  }
607 
632  template<
633  typename LieGroup_t,
634  typename Scalar,
635  int Options,
636  template<typename, int>
637  class JointCollectionTpl,
638  typename ConfigVectorType,
639  typename TangentVectorType,
640  typename JacobianMatrixType>
641  void dIntegrateTransport(
642  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
643  const Eigen::MatrixBase<ConfigVectorType> & q,
644  const Eigen::MatrixBase<TangentVectorType> & v,
645  const Eigen::MatrixBase<JacobianMatrixType> & J,
646  const ArgumentPosition arg);
647 
672  template<
673  typename Scalar,
674  int Options,
675  template<typename, int>
676  class JointCollectionTpl,
677  typename ConfigVectorType,
678  typename TangentVectorType,
679  typename JacobianMatrixType>
680  void dIntegrateTransport(
681  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
682  const Eigen::MatrixBase<ConfigVectorType> & q,
683  const Eigen::MatrixBase<TangentVectorType> & v,
684  const Eigen::MatrixBase<JacobianMatrixType> & J,
685  const ArgumentPosition arg)
686  {
687  dIntegrateTransport<
688  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
689  JacobianMatrixType>(
690  model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, J), arg);
691  }
692 
716  template<
717  typename LieGroup_t,
718  typename Scalar,
719  int Options,
720  template<typename, int>
721  class JointCollectionTpl,
722  typename ConfigVector1,
723  typename ConfigVector2,
724  typename JacobianMatrix>
725  void dDifference(
726  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
727  const Eigen::MatrixBase<ConfigVector1> & q0,
728  const Eigen::MatrixBase<ConfigVector2> & q1,
729  const Eigen::MatrixBase<JacobianMatrix> & J,
730  const ArgumentPosition arg);
731 
755  template<
756  typename Scalar,
757  int Options,
758  template<typename, int>
759  class JointCollectionTpl,
760  typename ConfigVector1,
761  typename ConfigVector2,
762  typename JacobianMatrix>
763  void dDifference(
764  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
765  const Eigen::MatrixBase<ConfigVector1> & q0,
766  const Eigen::MatrixBase<ConfigVector2> & q1,
767  const Eigen::MatrixBase<JacobianMatrix> & J,
768  const ArgumentPosition arg)
769  {
770  dDifference<
771  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVector1, ConfigVector2,
772  JacobianMatrix>(
773  model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, J), arg);
774  }
786  template<
787  typename LieGroup_t,
788  typename Scalar,
789  int Options,
790  template<typename, int>
791  class JointCollectionTpl,
792  typename ConfigVectorIn1,
793  typename ConfigVectorIn2>
794  Scalar squaredDistanceSum(
795  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
796  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
797  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
798 
811  template<
812  typename Scalar,
813  int Options,
814  template<typename, int>
815  class JointCollectionTpl,
816  typename ConfigVectorIn1,
817  typename ConfigVectorIn2>
818  Scalar squaredDistanceSum(
819  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
820  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
821  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
822  {
823  return squaredDistanceSum<
824  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
825  model, q0.derived(), q1.derived());
826  }
827 
840  template<
841  typename LieGroup_t,
842  typename Scalar,
843  int Options,
844  template<typename, int>
845  class JointCollectionTpl,
846  typename ConfigVectorIn1,
847  typename ConfigVectorIn2>
848  Scalar distance(
849  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
850  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
851  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
852 
864  template<
865  typename Scalar,
866  int Options,
867  template<typename, int>
868  class JointCollectionTpl,
869  typename ConfigVectorIn1,
870  typename ConfigVectorIn2>
871  Scalar distance(
872  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
873  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
874  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
875  {
876  return distance<
877  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
878  model, q0.derived(), q1.derived());
879  }
880 
889  template<
890  typename LieGroup_t,
891  typename Scalar,
892  int Options,
893  template<typename, int>
894  class JointCollectionTpl,
895  typename ConfigVectorType>
896  void normalize(
897  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
898  const Eigen::MatrixBase<ConfigVectorType> & qout);
899 
908  template<
909  typename Scalar,
910  int Options,
911  template<typename, int>
912  class JointCollectionTpl,
913  typename ConfigVectorType>
914  void normalize(
915  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
916  const Eigen::MatrixBase<ConfigVectorType> & qout)
917  {
918  normalize<LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType>(
919  model, PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, qout));
920  }
921 
933  template<
934  typename LieGroup_t,
935  typename Scalar,
936  int Options,
937  template<typename, int>
938  class JointCollectionTpl,
939  typename ConfigVectorType>
940  bool isNormalized(
941  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
942  const Eigen::MatrixBase<ConfigVectorType> & q,
943  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision());
944 
956  template<
957  typename Scalar,
958  int Options,
959  template<typename, int>
960  class JointCollectionTpl,
961  typename ConfigVectorType>
962  bool isNormalized(
963  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
964  const Eigen::MatrixBase<ConfigVectorType> & q,
965  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
966  {
967  return isNormalized<LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType>(
968  model, q, prec);
969  }
970 
987  template<
988  typename LieGroup_t,
989  typename Scalar,
990  int Options,
991  template<typename, int>
992  class JointCollectionTpl,
993  typename ConfigVectorIn1,
994  typename ConfigVectorIn2>
995  bool isSameConfiguration(
996  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
997  const Eigen::MatrixBase<ConfigVectorIn1> & q1,
998  const Eigen::MatrixBase<ConfigVectorIn2> & q2,
999  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision());
1000 
1017  template<
1018  typename Scalar,
1019  int Options,
1020  template<typename, int>
1021  class JointCollectionTpl,
1022  typename ConfigVectorIn1,
1023  typename ConfigVectorIn2>
1024  bool isSameConfiguration(
1025  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1026  const Eigen::MatrixBase<ConfigVectorIn1> & q1,
1027  const Eigen::MatrixBase<ConfigVectorIn2> & q2,
1028  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
1029  {
1030  return isSameConfiguration<
1031  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1032  model, q1.derived(), q2.derived(), prec);
1033  }
1034 
1047  template<
1048  typename LieGroup_t,
1049  typename Scalar,
1050  int Options,
1051  template<typename, int>
1052  class JointCollectionTpl,
1053  typename ConfigVector,
1054  typename JacobianMatrix>
1055  void integrateCoeffWiseJacobian(
1056  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1057  const Eigen::MatrixBase<ConfigVector> & q,
1058  const Eigen::MatrixBase<JacobianMatrix> & jacobian);
1059 
1072  template<
1073  typename Scalar,
1074  int Options,
1075  template<typename, int>
1076  class JointCollectionTpl,
1077  typename ConfigVector,
1078  typename JacobianMatrix>
1079  void integrateCoeffWiseJacobian(
1080  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1081  const Eigen::MatrixBase<ConfigVector> & q,
1082  const Eigen::MatrixBase<JacobianMatrix> & jacobian)
1083  {
1084  integrateCoeffWiseJacobian<
1085  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVector, JacobianMatrix>(
1086  model, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, jacobian));
1087  }
1088 
1090 
1093 
1107  template<
1108  typename LieGroup_t,
1109  typename Scalar,
1110  int Options,
1111  template<typename, int>
1112  class JointCollectionTpl,
1113  typename ConfigVectorType,
1114  typename TangentVectorType>
1115  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(
1116  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1117  const Eigen::MatrixBase<ConfigVectorType> & q,
1118  const Eigen::MatrixBase<TangentVectorType> & v);
1119 
1133  template<
1134  typename Scalar,
1135  int Options,
1136  template<typename, int>
1137  class JointCollectionTpl,
1138  typename ConfigVectorType,
1139  typename TangentVectorType>
1140  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(
1141  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1142  const Eigen::MatrixBase<ConfigVectorType> & q,
1143  const Eigen::MatrixBase<TangentVectorType> & v)
1144  {
1145  return integrate<
1146  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType>(
1147  model, q.derived(), v.derived());
1148  }
1149 
1163  template<
1164  typename LieGroup_t,
1165  typename Scalar,
1166  int Options,
1167  template<typename, int>
1168  class JointCollectionTpl,
1169  typename ConfigVectorIn1,
1170  typename ConfigVectorIn2>
1171  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) interpolate(
1172  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1173  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1174  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
1175  const Scalar & u);
1176 
1190  template<
1191  typename Scalar,
1192  int Options,
1193  template<typename, int>
1194  class JointCollectionTpl,
1195  typename ConfigVectorIn1,
1196  typename ConfigVectorIn2>
1197  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) interpolate(
1198  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1199  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1200  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
1201  const Scalar & u)
1202  {
1203  return interpolate<
1204  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1205  model, q0.derived(), q1.derived(), u);
1206  }
1207 
1220  template<
1221  typename LieGroup_t,
1222  typename Scalar,
1223  int Options,
1224  template<typename, int>
1225  class JointCollectionTpl,
1226  typename ConfigVectorIn1,
1227  typename ConfigVectorIn2>
1228  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) difference(
1229  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1230  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1231  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
1232 
1245  template<
1246  typename Scalar,
1247  int Options,
1248  template<typename, int>
1249  class JointCollectionTpl,
1250  typename ConfigVectorIn1,
1251  typename ConfigVectorIn2>
1252  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) difference(
1253  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1254  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1255  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
1256  {
1257  return difference<
1258  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1259  model, q0.derived(), q1.derived());
1260  }
1261 
1275  template<
1276  typename LieGroup_t,
1277  typename Scalar,
1278  int Options,
1279  template<typename, int>
1280  class JointCollectionTpl,
1281  typename ConfigVectorIn1,
1282  typename ConfigVectorIn2>
1283  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) squaredDistance(
1284  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1285  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1286  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
1287 
1301  template<
1302  typename Scalar,
1303  int Options,
1304  template<typename, int>
1305  class JointCollectionTpl,
1306  typename ConfigVectorIn1,
1307  typename ConfigVectorIn2>
1308  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) squaredDistance(
1309  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1310  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1311  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
1312  {
1313  return squaredDistance<
1314  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1315  model, q0.derived(), q1.derived());
1316  }
1317 
1336  template<
1337  typename LieGroup_t,
1338  typename Scalar,
1339  int Options,
1340  template<typename, int>
1341  class JointCollectionTpl,
1342  typename ConfigVectorIn1,
1343  typename ConfigVectorIn2>
1344  typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS(
1345  (typename ModelTpl<Scalar, Options, JointCollectionTpl>::ConfigVectorType))
1346  randomConfiguration(
1347  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1348  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
1349  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits);
1350 
1369  template<
1370  typename Scalar,
1371  int Options,
1372  template<typename, int>
1373  class JointCollectionTpl,
1374  typename ConfigVectorIn1,
1375  typename ConfigVectorIn2>
1376  typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS(
1377  (typename ModelTpl<Scalar, Options, JointCollectionTpl>::ConfigVectorType))
1378  randomConfiguration(
1379  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1380  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
1381  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits)
1382  {
1383  return randomConfiguration<
1384  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1385  model, lowerLimits.derived(), upperLimits.derived());
1386  }
1387 
1406  template<
1407  typename LieGroup_t,
1408  typename Scalar,
1409  int Options,
1410  template<typename, int>
1411  class JointCollectionTpl>
1412  typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS(
1413  (typename ModelTpl<Scalar, Options, JointCollectionTpl>::ConfigVectorType))
1414  randomConfiguration(const ModelTpl<Scalar, Options, JointCollectionTpl> & model);
1415 
1434  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
1435  typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS(
1436  (typename ModelTpl<Scalar, Options, JointCollectionTpl>::ConfigVectorType))
1437  randomConfiguration(const ModelTpl<Scalar, Options, JointCollectionTpl> & model)
1438  {
1439  return randomConfiguration<LieGroupMap, Scalar, Options, JointCollectionTpl>(model);
1440  }
1441 
1452  template<
1453  typename LieGroup_t,
1454  typename Scalar,
1455  int Options,
1456  template<typename, int>
1457  class JointCollectionTpl>
1458  Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options>
1459  neutral(const ModelTpl<Scalar, Options, JointCollectionTpl> & model);
1460 
1469  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
1470  Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options>
1471  neutral(const ModelTpl<Scalar, Options, JointCollectionTpl> & model)
1472  {
1473  return neutral<LieGroupMap, Scalar, Options, JointCollectionTpl>(model);
1474  }
1475 
1477 
1478 } // namespace pinocchio
1479 
1480 /* --- Details -------------------------------------------------------------------- */
1481 #include "pinocchio/algorithm/joint-configuration.hxx"
1482 
1483 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
1484  #include "pinocchio/algorithm/joint-configuration.txx"
1485 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
1486 
1487 #endif // ifndef __pinocchio_algorithm_joint_configuration_hpp__
cassie-simulation.qout
def qout
Definition: cassie-simulation.py:285
pinocchio::lowerLimits
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > & lowerLimits
Definition: joint-configuration.hpp:1348
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:871
pinocchio::SETTO
@ SETTO
Definition: fwd.hpp:132
pinocchio::u
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > const Scalar & u
Definition: joint-configuration.hpp:1175
pinocchio::Options
Options
Definition: joint-configuration.hpp:1116
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:502
PINOCCHIO_EIGEN_CONST_CAST
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
Definition: eigen-macros.hpp:51
inverse-kinematics.J
J
Definition: inverse-kinematics.py:31
model.hpp
q2
q2
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
pinocchio::ArgumentPosition
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: fwd.hpp:121
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:199
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:593
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:325
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:131
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:962
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:818
pinocchio::LieGroupMap
Definition: liegroup.hpp:17
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:72
pinocchio::q0
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > & q0
Definition: joint-configuration.hpp:1173
pinocchio::AssignmentOperatorType
AssignmentOperatorType
Definition: fwd.hpp:130
pinocchio::jacobian
void jacobian(const ConstraintModelTpl< Scalar, Options, ConstraintCollectionTpl > &cmodel, ConstraintDataTpl< Scalar, Options, ConstraintCollectionTpl > &cdata, const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< JacobianMatrix > &jacobian_matrix)
Definition: constraint-model-visitor.hpp:208
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1117
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1118
pinocchio::upperLimits
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & upperLimits
Definition: joint-configuration.hpp:1349
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:257
liegroup.hpp
pinocchio::q1
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & q1
Definition: joint-configuration.hpp:1174
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:763
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:375
pinocchio::PINOCCHIO_EIGEN_PLAIN_TYPE
PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(const ModelTpl< Scalar
Integrate a configuration vector for the specified model for a tangent vector during one unit time.
pinocchio::ModelTpl::ConfigVectorType
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Definition: multibody/model.hpp:88
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:1079
pinocchio::ModelTpl
Definition: context/generic.hpp:20
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:1024
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:914
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1116
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
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
Author(s):
autogenerated on Sat Jun 1 2024 02:40:35