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