pinocchio 4.1.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
Loading...
Searching...
No Matches
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
29namespace 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 }
1049
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
1256
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::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.
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...
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
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...
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.
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 .
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...
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.
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.
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::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::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.
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::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.
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...
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.
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.
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...
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.
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.
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.
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,...
pinocchio::is_lie_group_map
Helpers to use SFINAE for safe candidate selection compare to liegroup-variant-visitors....
Definition joint-configuration.hpp:35