$darkmode
pinocchio 4.0.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 //
6 // Copyright (c) INRIA 2026
7 //
8 #pragma once
9 
10 // IWYU pragma: begin_keep
11 #include <Eigen/Core>
12 
13 #include <cassert>
14 #include <vector>
15 
16 #include "pinocchio/macros.hpp"
17 #include "pinocchio/eigen-common.hpp"
18 #include "pinocchio/fwd.hpp"
19 #include "pinocchio/context.hpp"
20 
21 #include "pinocchio/utils/check.hpp"
22 
23 #include "pinocchio/math.hpp"
24 
25 #include "pinocchio/multibody.hpp"
26 #include "pinocchio/multibody/liegroup.hpp"
27 // IWYU pragma: end_keep
28 
29 namespace pinocchio
30 {
31 
33  template<typename T, typename = void>
34  struct is_lie_group_map : std::false_type
35  {
36  };
37 
38  // Pointer avoids instantiating operation<void>'s body (no complete type needed),
39  // while still detecting the nested template via SFINAE
40  template<typename T>
41  struct is_lie_group_map<T, std::void_t<typename T::template operation<void> *>> : std::true_type
42  {
43  };
44 
45  template<typename T>
46  inline constexpr bool is_lie_group_map_v = is_lie_group_map<T>::value;
47 
50 
67  template<
68  typename LieGroup_t,
69  typename Scalar,
70  int Options,
71  template<typename, int> class JointCollectionTpl,
72  typename ConfigVectorType,
73  typename TangentVectorType,
74  typename ReturnType,
75  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
76  void integrate(
77  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
78  const Eigen::MatrixBase<ConfigVectorType> & q,
79  const Eigen::MatrixBase<TangentVectorType> & v,
80  const Eigen::MatrixBase<ReturnType> & qout);
81 
98  template<
99  typename Scalar,
100  int Options,
101  template<typename, int> class JointCollectionTpl,
102  typename ConfigVectorType,
103  typename TangentVectorType,
104  typename ReturnType>
105  void integrate(
106  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
107  const Eigen::MatrixBase<ConfigVectorType> & q,
108  const Eigen::MatrixBase<TangentVectorType> & v,
109  const Eigen::MatrixBase<ReturnType> & qout)
110  {
111  integrate<
112  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
113  ReturnType>(model, q.derived(), v.derived(), qout.derived());
114  }
115 
128  template<
129  typename LieGroup_t,
130  typename Scalar,
131  int Options,
132  template<typename, int> class JointCollectionTpl,
133  typename ConfigVectorIn1,
134  typename ConfigVectorIn2,
135  typename ReturnType,
136  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
137  void interpolate(
138  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
139  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
140  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
141  const Scalar & u,
142  const Eigen::MatrixBase<ReturnType> & qout);
143 
156  template<
157  typename Scalar,
158  int Options,
159  template<typename, int> class JointCollectionTpl,
160  typename ConfigVectorIn1,
161  typename ConfigVectorIn2,
162  typename ReturnType>
163  void interpolate(
164  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
165  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
166  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
167  const Scalar & u,
168  const Eigen::MatrixBase<ReturnType> & qout)
169  {
170  interpolate<
171  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
172  ReturnType>(
173  model, q0.derived(), q1.derived(), u, PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout));
174  }
175 
192  template<
193  typename LieGroup_t,
194  typename Scalar,
195  int Options,
196  template<typename, int> class JointCollectionTpl,
197  typename ConfigVectorIn1,
198  typename ConfigVectorIn2,
199  typename ReturnType,
200  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
201  void difference(
202  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
203  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
204  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
205  const Eigen::MatrixBase<ReturnType> & dvout);
206 
223  template<
224  typename Scalar,
225  int Options,
226  template<typename, int> class JointCollectionTpl,
227  typename ConfigVectorIn1,
228  typename ConfigVectorIn2,
229  typename ReturnType>
230  void difference(
231  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
232  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
233  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
234  const Eigen::MatrixBase<ReturnType> & dvout)
235  {
236  difference<
237  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
238  ReturnType>(model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(ReturnType, dvout));
239  }
240 
253  template<
254  typename LieGroup_t,
255  typename Scalar,
256  int Options,
257  template<typename, int> class JointCollectionTpl,
258  typename ConfigVectorIn1,
259  typename ConfigVectorIn2,
260  typename ReturnType,
261  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
262  void squaredDistance(
263  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
264  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
265  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
266  const Eigen::MatrixBase<ReturnType> & out);
267 
280  template<
281  typename Scalar,
282  int Options,
283  template<typename, int> class JointCollectionTpl,
284  typename ConfigVectorIn1,
285  typename ConfigVectorIn2,
286  typename ReturnType>
287  void squaredDistance(
288  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
289  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
290  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
291  const Eigen::MatrixBase<ReturnType> & out)
292  {
293  squaredDistance<
294  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
295  ReturnType>(model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(ReturnType, out));
296  }
297 
315  template<
316  typename LieGroup_t,
317  typename Scalar,
318  int Options,
319  template<typename, int> class JointCollectionTpl,
320  typename ConfigVectorIn1,
321  typename ConfigVectorIn2,
322  typename ReturnType,
323  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
324  void randomConfiguration(
325  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
326  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
327  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
328  const Eigen::MatrixBase<ReturnType> & qout);
329 
347  template<
348  typename Scalar,
349  int Options,
350  template<typename, int> class JointCollectionTpl,
351  typename ConfigVectorIn1,
352  typename ConfigVectorIn2,
353  typename ReturnType>
354  void randomConfiguration(
355  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
356  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
357  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
358  const Eigen::MatrixBase<ReturnType> & qout)
359  {
360  randomConfiguration<
361  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
362  ReturnType>(
363  model, lowerLimits.derived(), upperLimits.derived(),
364  PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout));
365  }
366 
377  template<
378  typename LieGroup_t,
379  typename Scalar,
380  int Options,
381  template<typename, int> class JointCollectionTpl,
382  typename ReturnType,
383  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
384  void neutral(
385  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
386  const Eigen::MatrixBase<ReturnType> & qout);
387 
398  template<
399  typename Scalar,
400  int Options,
401  template<typename, int> class JointCollectionTpl,
402  typename ReturnType>
403  void neutral(
404  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
405  const Eigen::MatrixBase<ReturnType> & qout)
406  {
407  neutral<LieGroupMap, Scalar, Options, JointCollectionTpl, ReturnType>(
408  model, PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout));
409  }
410 
436  template<
437  typename LieGroup_t,
438  typename Scalar,
439  int Options,
440  template<typename, int> class JointCollectionTpl,
441  typename ConfigVectorType,
442  typename TangentVectorType,
443  typename JacobianMatrixType,
444  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
445  void dIntegrate(
446  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
447  const Eigen::MatrixBase<ConfigVectorType> & q,
448  const Eigen::MatrixBase<TangentVectorType> & v,
449  const Eigen::MatrixBase<JacobianMatrixType> & J,
450  const ArgumentPosition arg,
451  const AssignmentOperatorType op = SETTO);
452 
477  template<
478  typename Scalar,
479  int Options,
480  template<typename, int> class JointCollectionTpl,
481  typename ConfigVectorType,
482  typename TangentVectorType,
483  typename JacobianMatrixType>
484  void dIntegrate(
485  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
486  const Eigen::MatrixBase<ConfigVectorType> & q,
487  const Eigen::MatrixBase<TangentVectorType> & v,
488  const Eigen::MatrixBase<JacobianMatrixType> & J,
489  const ArgumentPosition arg)
490  {
491  dIntegrate<
492  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
493  JacobianMatrixType>(
494  model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, J), arg,
495  SETTO);
496  }
497 
523  template<
524  typename Scalar,
525  int Options,
526  template<typename, int> class JointCollectionTpl,
527  typename ConfigVectorType,
528  typename TangentVectorType,
529  typename JacobianMatrixType>
530  void dIntegrate(
531  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
532  const Eigen::MatrixBase<ConfigVectorType> & q,
533  const Eigen::MatrixBase<TangentVectorType> & v,
534  const Eigen::MatrixBase<JacobianMatrixType> & J,
535  const ArgumentPosition arg,
536  const AssignmentOperatorType op)
537  {
538  dIntegrate<
539  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
540  JacobianMatrixType>(
541  model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, J), arg, op);
542  }
543 
562  template<
563  typename LieGroup_t,
564  typename Scalar,
565  int Options,
566  template<typename, int> class JointCollectionTpl,
567  typename ConfigVectorType,
568  typename TangentMapMatrixType,
569  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
570  void tangentMap(
571  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
572  const Eigen::MatrixBase<ConfigVectorType> & q,
573  const Eigen::MatrixBase<TangentMapMatrixType> & TM,
574  const AssignmentOperatorType op = SETTO);
575 
594  template<
595  typename Scalar,
596  int Options,
597  template<typename, int> class JointCollectionTpl,
598  typename ConfigVectorType,
599  typename TangentMapMatrixType>
600  void tangentMap(
601  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
602  const Eigen::MatrixBase<ConfigVectorType> & q,
603  const Eigen::MatrixBase<TangentMapMatrixType> & TM,
604  const AssignmentOperatorType op = SETTO)
605  {
606  tangentMap<
607  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentMapMatrixType>(
608  model, q.derived(), TM.const_cast_derived(), op);
609  }
610 
623  template<
624  typename LieGroup_t,
625  typename Scalar,
626  int Options,
627  template<typename, int> class JointCollectionTpl,
628  typename ConfigVectorType,
629  typename TangentMapMatrixType>
630  void compactTangentMap(
631  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
632  const std::vector<JointIndex> & joint_ids,
633  const Eigen::MatrixBase<ConfigVectorType> & q,
634  const Eigen::MatrixBase<TangentMapMatrixType> & TMc);
635 
648  template<
649  typename Scalar,
650  int Options,
651  template<typename, int> class JointCollectionTpl,
652  typename ConfigVectorType,
653  typename TangentMapMatrixType>
654  void compactTangentMap(
655  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
656  const std::vector<JointIndex> & joint_ids,
657  const Eigen::MatrixBase<ConfigVectorType> & q,
658  const Eigen::MatrixBase<TangentMapMatrixType> & TMc)
659  {
660  compactTangentMap<
661  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentMapMatrixType>(
662  model, joint_ids, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(TangentMapMatrixType, TMc));
663  }
664 
679  template<
680  typename LieGroup_t,
681  typename Scalar,
682  int Options,
683  template<typename, int> class JointCollectionTpl,
684  typename ConfigVectorType,
685  typename MatrixInType,
686  typename MatrixOutType,
687  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
688  void tangentMapProduct(
689  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
690  const Eigen::MatrixBase<ConfigVectorType> & q,
691  const Eigen::MatrixBase<MatrixInType> & mat_in,
692  const Eigen::MatrixBase<MatrixOutType> & mat_out,
693  const AssignmentOperatorType op = SETTO);
694 
709  template<
710  typename Scalar,
711  int Options,
712  template<typename, int> class JointCollectionTpl,
713  typename ConfigVectorType,
714  typename MatrixInType,
715  typename MatrixOutType>
716  void tangentMapProduct(
717  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
718  const Eigen::MatrixBase<ConfigVectorType> & q,
719  const Eigen::MatrixBase<MatrixInType> & mat_in,
720  const Eigen::MatrixBase<MatrixOutType> & mat_out,
721  const AssignmentOperatorType op = SETTO)
722  {
723  tangentMapProduct<
724  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, MatrixInType,
725  MatrixOutType>(
726  model, q.derived(), mat_in.derived(), PINOCCHIO_EIGEN_CONST_CAST(MatrixOutType, mat_out), op);
727  }
728 
743  template<
744  typename LieGroup_t,
745  typename Scalar,
746  int Options,
747  template<typename, int> class JointCollectionTpl,
748  typename ConfigVectorType,
749  typename MatrixInType,
750  typename MatrixOutType,
751  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
752  void tangentMapTransposeProduct(
753  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
754  const Eigen::MatrixBase<ConfigVectorType> & q,
755  const Eigen::MatrixBase<MatrixInType> & mat_in,
756  const Eigen::MatrixBase<MatrixOutType> & mat_out,
757  const AssignmentOperatorType op = SETTO);
758 
773  template<
774  typename Scalar,
775  int Options,
776  template<typename, int> class JointCollectionTpl,
777  typename ConfigVectorType,
778  typename MatrixInType,
779  typename MatrixOutType>
780  void tangentMapTransposeProduct(
781  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
782  const Eigen::MatrixBase<ConfigVectorType> & q,
783  const Eigen::MatrixBase<MatrixInType> & mat_in,
784  const Eigen::MatrixBase<MatrixOutType> & mat_out,
785  const AssignmentOperatorType op = SETTO)
786  {
787  tangentMapTransposeProduct<
788  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, MatrixInType,
789  MatrixOutType>(
790  model, q.derived(), mat_in.derived(), PINOCCHIO_EIGEN_CONST_CAST(MatrixOutType, mat_out), op);
791  }
792 
818  template<
819  typename LieGroup_t,
820  typename Scalar,
821  int Options,
822  template<typename, int> class JointCollectionTpl,
823  typename ConfigVectorType,
824  typename TangentVectorType,
825  typename JacobianMatrixType1,
826  typename JacobianMatrixType2,
827  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
828  void dIntegrateTransport(
829  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
830  const Eigen::MatrixBase<ConfigVectorType> & q,
831  const Eigen::MatrixBase<TangentVectorType> & v,
832  const Eigen::MatrixBase<JacobianMatrixType1> & Jin,
833  const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
834  const ArgumentPosition arg);
835 
861  template<
862  typename Scalar,
863  int Options,
864  template<typename, int> class JointCollectionTpl,
865  typename ConfigVectorType,
866  typename TangentVectorType,
867  typename JacobianMatrixType1,
868  typename JacobianMatrixType2>
869  void dIntegrateTransport(
870  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
871  const Eigen::MatrixBase<ConfigVectorType> & q,
872  const Eigen::MatrixBase<TangentVectorType> & v,
873  const Eigen::MatrixBase<JacobianMatrixType1> & Jin,
874  const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
875  const ArgumentPosition arg)
876  {
877  dIntegrateTransport<
878  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
879  JacobianMatrixType1, JacobianMatrixType2>(
880  model, q.derived(), v.derived(), Jin.derived(),
881  PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType2, Jout), arg);
882  }
883 
908  template<
909  typename LieGroup_t,
910  typename Scalar,
911  int Options,
912  template<typename, int> class JointCollectionTpl,
913  typename ConfigVectorType,
914  typename TangentVectorType,
915  typename JacobianMatrixType,
916  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
917  void dIntegrateTransport(
918  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
919  const Eigen::MatrixBase<ConfigVectorType> & q,
920  const Eigen::MatrixBase<TangentVectorType> & v,
921  const Eigen::MatrixBase<JacobianMatrixType> & J,
922  const ArgumentPosition arg);
923 
948  template<
949  typename Scalar,
950  int Options,
951  template<typename, int> class JointCollectionTpl,
952  typename ConfigVectorType,
953  typename TangentVectorType,
954  typename JacobianMatrixType>
955  void dIntegrateTransport(
956  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
957  const Eigen::MatrixBase<ConfigVectorType> & q,
958  const Eigen::MatrixBase<TangentVectorType> & v,
959  const Eigen::MatrixBase<JacobianMatrixType> & J,
960  const ArgumentPosition arg)
961  {
962  dIntegrateTransport<
963  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
964  JacobianMatrixType>(
965  model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, J), arg);
966  }
967 
991  template<
992  typename LieGroup_t,
993  typename Scalar,
994  int Options,
995  template<typename, int> class JointCollectionTpl,
996  typename ConfigVector1,
997  typename ConfigVector2,
998  typename JacobianMatrix,
999  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
1000  void dDifference(
1001  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1002  const Eigen::MatrixBase<ConfigVector1> & q0,
1003  const Eigen::MatrixBase<ConfigVector2> & q1,
1004  const Eigen::MatrixBase<JacobianMatrix> & J,
1005  const ArgumentPosition arg);
1006 
1030  template<
1031  typename Scalar,
1032  int Options,
1033  template<typename, int> class JointCollectionTpl,
1034  typename ConfigVector1,
1035  typename ConfigVector2,
1036  typename JacobianMatrix>
1037  void dDifference(
1038  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1039  const Eigen::MatrixBase<ConfigVector1> & q0,
1040  const Eigen::MatrixBase<ConfigVector2> & q1,
1041  const Eigen::MatrixBase<JacobianMatrix> & J,
1042  const ArgumentPosition arg)
1043  {
1044  dDifference<
1045  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVector1, ConfigVector2,
1046  JacobianMatrix>(
1047  model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, J), arg);
1048  }
1060  template<
1061  typename LieGroup_t,
1062  typename Scalar,
1063  int Options,
1064  template<typename, int> class JointCollectionTpl,
1065  typename ConfigVectorIn1,
1066  typename ConfigVectorIn2>
1067  Scalar squaredDistanceSum(
1068  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1069  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1070  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
1071 
1084  template<
1085  typename Scalar,
1086  int Options,
1087  template<typename, int> class JointCollectionTpl,
1088  typename ConfigVectorIn1,
1089  typename ConfigVectorIn2>
1090  Scalar squaredDistanceSum(
1091  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1092  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1093  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
1094  {
1095  return squaredDistanceSum<
1096  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1097  model, q0.derived(), q1.derived());
1098  }
1099 
1112  template<
1113  typename LieGroup_t,
1114  typename Scalar,
1115  int Options,
1116  template<typename, int> class JointCollectionTpl,
1117  typename ConfigVectorIn1,
1118  typename ConfigVectorIn2,
1119  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
1120  Scalar distance(
1121  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1122  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1123  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
1124 
1136  template<
1137  typename Scalar,
1138  int Options,
1139  template<typename, int> class JointCollectionTpl,
1140  typename ConfigVectorIn1,
1141  typename ConfigVectorIn2>
1142  Scalar distance(
1143  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1144  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1145  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
1146  {
1147  return distance<
1148  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1149  model, q0.derived(), q1.derived());
1150  }
1151 
1160  template<
1161  typename LieGroup_t,
1162  typename Scalar,
1163  int Options,
1164  template<typename, int> class JointCollectionTpl,
1165  typename ConfigVectorType,
1166  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
1167  void normalize(
1168  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1169  const Eigen::MatrixBase<ConfigVectorType> & qout);
1170 
1179  template<
1180  typename Scalar,
1181  int Options,
1182  template<typename, int> class JointCollectionTpl,
1183  typename ConfigVectorType>
1184  void normalize(
1185  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1186  const Eigen::MatrixBase<ConfigVectorType> & qout)
1187  {
1188  normalize<LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType>(
1189  model, PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, qout));
1190  }
1191 
1203  template<
1204  typename LieGroup_t,
1205  typename Scalar,
1206  int Options,
1207  template<typename, int> class JointCollectionTpl,
1208  typename ConfigVectorType,
1209  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
1210  bool isNormalized(
1211  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1212  const Eigen::MatrixBase<ConfigVectorType> & q,
1213  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision());
1214 
1226  template<
1227  typename Scalar,
1228  int Options,
1229  template<typename, int> class JointCollectionTpl,
1230  typename ConfigVectorType>
1231  bool isNormalized(
1232  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1233  const Eigen::MatrixBase<ConfigVectorType> & q,
1234  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
1235  {
1236  return isNormalized<LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType>(
1237  model, q, prec);
1238  }
1239 
1257  template<
1258  typename LieGroup_t,
1259  typename Scalar,
1260  int Options,
1261  template<typename, int> class JointCollectionTpl,
1262  typename ConfigVectorIn1,
1263  typename ConfigVectorIn2,
1264  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
1265  bool isSameConfiguration(
1266  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1267  const Eigen::MatrixBase<ConfigVectorIn1> & q1,
1268  const Eigen::MatrixBase<ConfigVectorIn2> & q2,
1269  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision());
1270 
1287  template<
1288  typename Scalar,
1289  int Options,
1290  template<typename, int> class JointCollectionTpl,
1291  typename ConfigVectorIn1,
1292  typename ConfigVectorIn2>
1293  bool isSameConfiguration(
1294  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1295  const Eigen::MatrixBase<ConfigVectorIn1> & q1,
1296  const Eigen::MatrixBase<ConfigVectorIn2> & q2,
1297  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
1298  {
1299  return isSameConfiguration<
1300  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1301  model, q1.derived(), q2.derived(), prec);
1302  }
1303 
1317  template<
1318  typename LieGroup_t,
1319  typename Scalar,
1320  int Options,
1321  template<typename, int> class JointCollectionTpl,
1322  typename ConfigVector,
1323  typename JacobianMatrix>
1324  void integrateCoeffWiseJacobian(
1325  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1326  const Eigen::MatrixBase<ConfigVector> & q,
1327  const Eigen::MatrixBase<JacobianMatrix> & jacobian);
1328 
1342  template<
1343  typename Scalar,
1344  int Options,
1345  template<typename, int> class JointCollectionTpl,
1346  typename ConfigVector,
1347  typename JacobianMatrix>
1348  void integrateCoeffWiseJacobian(
1349  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1350  const Eigen::MatrixBase<ConfigVector> & q,
1351  const Eigen::MatrixBase<JacobianMatrix> & jacobian)
1352  {
1353  integrateCoeffWiseJacobian<
1354  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVector, JacobianMatrix>(
1355  model, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, jacobian));
1356  }
1357 
1367  template<
1368  typename LieGroup_t,
1369  typename Scalar,
1370  int Options,
1371  template<typename, int> class JointCollectionTpl>
1372  void lieGroup(
1373  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1374  typename LieGroup_t::template operationProduct<Scalar, Options>::type & lgo);
1375 
1385  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
1386  void lieGroup(
1387  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1388  typename LieGroupMap::template operationProduct<Scalar, Options>::type & lgo)
1389  {
1390  lieGroup<LieGroupMap, Scalar, Options, JointCollectionTpl>(model, lgo);
1391  }
1392 
1407  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
1408  void getTangentToConfigurationSparsitySegment(
1409  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1410  const std::vector<JointIndex> & joint_ids,
1411  std::vector<int> & nvs,
1412  std::vector<int> & idx_vs);
1413 
1415 
1418 
1432  template<
1433  typename LieGroup_t,
1434  typename Scalar,
1435  int Options,
1436  template<typename, int> class JointCollectionTpl,
1437  typename ConfigVectorType,
1438  typename TangentVectorType,
1439  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
1440  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(
1441  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1442  const Eigen::MatrixBase<ConfigVectorType> & q,
1443  const Eigen::MatrixBase<TangentVectorType> & v);
1444 
1458  template<
1459  typename Scalar,
1460  int Options,
1461  template<typename, int> class JointCollectionTpl,
1462  typename ConfigVectorType,
1463  typename TangentVectorType>
1464  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(
1465  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1466  const Eigen::MatrixBase<ConfigVectorType> & q,
1467  const Eigen::MatrixBase<TangentVectorType> & v)
1468  {
1469  return integrate<
1470  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType>(
1471  model, q.derived(), v.derived());
1472  }
1473 
1487  template<
1488  typename LieGroup_t,
1489  typename Scalar,
1490  int Options,
1491  template<typename, int> class JointCollectionTpl,
1492  typename ConfigVectorIn1,
1493  typename ConfigVectorIn2,
1494  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
1495  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) interpolate(
1496  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1497  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1498  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
1499  const Scalar & u);
1500 
1514  template<
1515  typename Scalar,
1516  int Options,
1517  template<typename, int> class JointCollectionTpl,
1518  typename ConfigVectorIn1,
1519  typename ConfigVectorIn2>
1520  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) interpolate(
1521  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1522  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1523  const Eigen::MatrixBase<ConfigVectorIn2> & q1,
1524  const Scalar & u)
1525  {
1526  return interpolate<
1527  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1528  model, q0.derived(), q1.derived(), u);
1529  }
1530 
1543  template<
1544  typename LieGroup_t,
1545  typename Scalar,
1546  int Options,
1547  template<typename, int> class JointCollectionTpl,
1548  typename ConfigVectorIn1,
1549  typename ConfigVectorIn2,
1550  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
1551  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) difference(
1552  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1553  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1554  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
1555 
1568  template<
1569  typename Scalar,
1570  int Options,
1571  template<typename, int> class JointCollectionTpl,
1572  typename ConfigVectorIn1,
1573  typename ConfigVectorIn2>
1574  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) difference(
1575  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1576  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1577  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
1578  {
1579  return difference<
1580  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1581  model, q0.derived(), q1.derived());
1582  }
1583 
1597  template<
1598  typename LieGroup_t,
1599  typename Scalar,
1600  int Options,
1601  template<typename, int> class JointCollectionTpl,
1602  typename ConfigVectorIn1,
1603  typename ConfigVectorIn2,
1604  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
1605  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) squaredDistance(
1606  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1607  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1608  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
1609 
1623  template<
1624  typename Scalar,
1625  int Options,
1626  template<typename, int> class JointCollectionTpl,
1627  typename ConfigVectorIn1,
1628  typename ConfigVectorIn2>
1629  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) squaredDistance(
1630  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1631  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1632  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
1633  {
1634  return squaredDistance<
1635  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1636  model, q0.derived(), q1.derived());
1637  }
1638 
1657  template<
1658  typename LieGroup_t,
1659  typename Scalar,
1660  int Options,
1661  template<typename, int> class JointCollectionTpl,
1662  typename ConfigVectorIn1,
1663  typename ConfigVectorIn2,
1664  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
1665  typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS(
1666  (typename ModelTpl<Scalar, Options, JointCollectionTpl>::ConfigVectorType))
1667  randomConfiguration(
1668  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1669  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
1670  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits);
1671 
1690  template<
1691  typename Scalar,
1692  int Options,
1693  template<typename, int> class JointCollectionTpl,
1694  typename ConfigVectorIn1,
1695  typename ConfigVectorIn2>
1696  typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS(
1697  (typename ModelTpl<Scalar, Options, JointCollectionTpl>::ConfigVectorType))
1698  randomConfiguration(
1699  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1700  const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
1701  const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits)
1702  {
1703  return randomConfiguration<
1704  LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1705  model, lowerLimits.derived(), upperLimits.derived());
1706  }
1707 
1726  template<
1727  typename LieGroup_t,
1728  typename Scalar,
1729  int Options,
1730  template<typename, int> class JointCollectionTpl,
1731  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
1732  typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS(
1733  (typename ModelTpl<Scalar, Options, JointCollectionTpl>::ConfigVectorType))
1734  randomConfiguration(const ModelTpl<Scalar, Options, JointCollectionTpl> & model);
1735 
1754  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
1755  typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS(
1756  (typename ModelTpl<Scalar, Options, JointCollectionTpl>::ConfigVectorType))
1757  randomConfiguration(const ModelTpl<Scalar, Options, JointCollectionTpl> & model)
1758  {
1759  return randomConfiguration<LieGroupMap, Scalar, Options, JointCollectionTpl>(model);
1760  }
1761 
1772  template<
1773  typename LieGroup_t,
1774  typename Scalar,
1775  int Options,
1776  template<typename, int> class JointCollectionTpl,
1777  std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
1778  Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options>
1779  neutral(const ModelTpl<Scalar, Options, JointCollectionTpl> & model);
1780 
1789  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
1790  Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options>
1791  neutral(const ModelTpl<Scalar, Options, JointCollectionTpl> & model)
1792  {
1793  return neutral<LieGroupMap, Scalar, Options, JointCollectionTpl>(model);
1794  }
1795 
1805  template<
1806  typename LieGroup_t,
1807  typename Scalar,
1808  int Options,
1809  template<typename, int> class JointCollectionTpl>
1810  typename LieGroup_t::template operationProduct<Scalar, Options>::type
1811  lieGroup(const ModelTpl<Scalar, Options, JointCollectionTpl> & model);
1812 
1822  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
1823  typename LieGroupMap::template operationProduct<Scalar, Options>::type
1824  lieGroup(const ModelTpl<Scalar, Options, JointCollectionTpl> & model)
1825  {
1826  return lieGroup<LieGroupMap, Scalar, Options, JointCollectionTpl>(model);
1827  }
1828 
1830 
1831 } // namespace pinocchio
1832 
1833 // IWYU pragma: begin_exports
1834 #include "pinocchio/src/algorithm/joint-configuration.hxx"
1835 // IWYU pragma: end_exports
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::tangentMapProduct
void tangentMapProduct(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< MatrixInType > &mat_in, const Eigen::MatrixBase< MatrixOutType > &mat_out, const AssignmentOperatorType op=SETTO)
Compose the tangentMap with a matrix, e.g., a Lie group Jacobian in order to recover a vector space J...
Definition: joint-configuration.hpp:716
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:287
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:1231
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:869
pinocchio::compactTangentMap
void compactTangentMap(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const std::vector< JointIndex > &joint_ids, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentMapMatrixType > &TMc)
Set the tangentMap in a compact manner in matric of size nq x MAX_JOINT_NV.
Definition: joint-configuration.hpp:654
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:1293
pinocchio::lieGroup
void lieGroup(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, typename LieGroup_t::template operationProduct< Scalar, Options >::type &lgo)
Returns the Lie group associated to the model. It is the cartesian product of the lie groups of all i...
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:105
pinocchio::tangentMap
void tangentMap(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentMapMatrixType > &TM, const AssignmentOperatorType op=SETTO)
Computes the tangentMap that map a small variation of the configuration express in the Lie algebra (a...
Definition: joint-configuration.hpp:600
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:230
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::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:1090
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:1037
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:1348
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:403
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:163
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:1142
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:530
pinocchio::getTangentToConfigurationSparsitySegment
void getTangentToConfigurationSparsitySegment(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const std::vector< JointIndex > &joint_ids, std::vector< int > &nvs, std::vector< int > &idx_vs)
Return two vector where for each, the idx_v and v associated to the same atomic joint is given.
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:1184
pinocchio::tangentMapTransposeProduct
void tangentMapTransposeProduct(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< MatrixInType > &mat_in, const Eigen::MatrixBase< MatrixOutType > &mat_out, const AssignmentOperatorType op=SETTO)
Compose the tangentMap with a matrix, e.g., a Lie group Jacobian in order to recover a vector space J...
Definition: joint-configuration.hpp:780
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:354
pinocchio::is_lie_group_map
Helpers to use SFINAE for safe candidate selection compare to liegroup-variant-visitors....
Definition: joint-configuration.hpp:35