5 #ifndef __pinocchio_algorithm_joint_configuration_hpp__
6 #define __pinocchio_algorithm_joint_configuration_hpp__
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/multibody/liegroup/liegroup.hpp"
37 template<
typename,
int>
38 class JointCollectionTpl,
39 typename ConfigVectorType,
40 typename TangentVectorType,
43 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
44 const Eigen::MatrixBase<ConfigVectorType> & q,
45 const Eigen::MatrixBase<TangentVectorType> & v,
46 const Eigen::MatrixBase<ReturnType> & qout);
67 template<
typename,
int>
68 class JointCollectionTpl,
69 typename ConfigVectorType,
70 typename TangentVectorType,
74 const Eigen::MatrixBase<ConfigVectorType> & q,
75 const Eigen::MatrixBase<TangentVectorType> & v,
76 const Eigen::MatrixBase<ReturnType> & qout)
79 LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
80 ReturnType>(model, q.derived(), v.derived(), qout.derived());
99 template<
typename,
int>
100 class JointCollectionTpl,
101 typename ConfigVectorIn1,
102 typename ConfigVectorIn2,
105 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
106 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
107 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
109 const Eigen::MatrixBase<ReturnType> & qout);
126 template<
typename,
int>
127 class JointCollectionTpl,
128 typename ConfigVectorIn1,
129 typename ConfigVectorIn2,
133 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
134 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
136 const Eigen::MatrixBase<ReturnType> & qout)
139 LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
141 model, q0.derived(), q1.derived(), u, PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout));
164 template<
typename,
int>
165 class JointCollectionTpl,
166 typename ConfigVectorIn1,
167 typename ConfigVectorIn2,
170 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
171 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
172 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
173 const Eigen::MatrixBase<ReturnType> & dvout);
194 template<
typename,
int>
195 class JointCollectionTpl,
196 typename ConfigVectorIn1,
197 typename ConfigVectorIn2,
201 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
202 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
203 const Eigen::MatrixBase<ReturnType> & dvout)
206 LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
207 ReturnType>(model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(ReturnType, dvout));
226 template<
typename,
int>
227 class JointCollectionTpl,
228 typename ConfigVectorIn1,
229 typename ConfigVectorIn2,
232 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
233 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
234 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
235 const Eigen::MatrixBase<ReturnType> & out);
252 template<
typename,
int>
253 class JointCollectionTpl,
254 typename ConfigVectorIn1,
255 typename ConfigVectorIn2,
259 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
260 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
261 const Eigen::MatrixBase<ReturnType> & out)
264 LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
265 ReturnType>(model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(ReturnType, out));
289 template<
typename,
int>
290 class JointCollectionTpl,
291 typename ConfigVectorIn1,
292 typename ConfigVectorIn2,
295 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
296 const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
297 const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
298 const Eigen::MatrixBase<ReturnType> & qout);
320 template<
typename,
int>
321 class JointCollectionTpl,
322 typename ConfigVectorIn1,
323 typename ConfigVectorIn2,
327 const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
328 const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
329 const Eigen::MatrixBase<ReturnType> & qout)
332 LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
334 model, lowerLimits.derived(), upperLimits.derived(),
335 PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout));
352 template<
typename,
int>
353 class JointCollectionTpl,
356 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
357 const Eigen::MatrixBase<ReturnType> & qout);
372 template<
typename,
int>
373 class JointCollectionTpl,
377 const Eigen::MatrixBase<ReturnType> & qout)
379 neutral<LieGroupMap, Scalar, Options, JointCollectionTpl, ReturnType>(
380 model, PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout));
411 template<
typename,
int>
412 class JointCollectionTpl,
413 typename ConfigVectorType,
414 typename TangentVectorType,
415 typename JacobianMatrixType>
417 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
418 const Eigen::MatrixBase<ConfigVectorType> & q,
419 const Eigen::MatrixBase<TangentVectorType> & v,
420 const Eigen::MatrixBase<JacobianMatrixType> & J,
422 const AssignmentOperatorType op = SETTO);
451 template<
typename,
int>
452 class JointCollectionTpl,
453 typename ConfigVectorType,
454 typename TangentVectorType,
455 typename JacobianMatrixType>
458 const Eigen::MatrixBase<ConfigVectorType> & q,
459 const Eigen::MatrixBase<TangentVectorType> & v,
460 const Eigen::MatrixBase<JacobianMatrixType> & J,
464 LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
466 model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, J), arg,
497 template<
typename,
int>
498 class JointCollectionTpl,
499 typename ConfigVectorType,
500 typename TangentVectorType,
501 typename JacobianMatrixType>
504 const Eigen::MatrixBase<ConfigVectorType> & q,
505 const Eigen::MatrixBase<TangentVectorType> & v,
506 const Eigen::MatrixBase<JacobianMatrixType> & J,
508 const AssignmentOperatorType op)
511 LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
513 model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, J), arg, op);
545 template<
typename,
int>
546 class JointCollectionTpl,
547 typename ConfigVectorType,
548 typename TangentVectorType,
549 typename JacobianMatrixType1,
550 typename JacobianMatrixType2>
552 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
553 const Eigen::MatrixBase<ConfigVectorType> & q,
554 const Eigen::MatrixBase<TangentVectorType> & v,
555 const Eigen::MatrixBase<JacobianMatrixType1> & Jin,
556 const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
587 template<
typename,
int>
588 class JointCollectionTpl,
589 typename ConfigVectorType,
590 typename TangentVectorType,
591 typename JacobianMatrixType1,
592 typename JacobianMatrixType2>
595 const Eigen::MatrixBase<ConfigVectorType> & q,
596 const Eigen::MatrixBase<TangentVectorType> & v,
597 const Eigen::MatrixBase<JacobianMatrixType1> & Jin,
598 const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
602 LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
603 JacobianMatrixType1, JacobianMatrixType2>(
604 model, q.derived(), v.derived(), Jin.derived(),
605 PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType2, Jout), arg);
636 template<
typename,
int>
637 class JointCollectionTpl,
638 typename ConfigVectorType,
639 typename TangentVectorType,
640 typename JacobianMatrixType>
642 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
643 const Eigen::MatrixBase<ConfigVectorType> & q,
644 const Eigen::MatrixBase<TangentVectorType> & v,
645 const Eigen::MatrixBase<JacobianMatrixType> & J,
675 template<
typename,
int>
676 class JointCollectionTpl,
677 typename ConfigVectorType,
678 typename TangentVectorType,
679 typename JacobianMatrixType>
682 const Eigen::MatrixBase<ConfigVectorType> & q,
683 const Eigen::MatrixBase<TangentVectorType> & v,
684 const Eigen::MatrixBase<JacobianMatrixType> & J,
688 LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
690 model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, J), arg);
720 template<
typename,
int>
721 class JointCollectionTpl,
722 typename ConfigVector1,
723 typename ConfigVector2,
724 typename JacobianMatrix>
726 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
727 const Eigen::MatrixBase<ConfigVector1> & q0,
728 const Eigen::MatrixBase<ConfigVector2> & q1,
729 const Eigen::MatrixBase<JacobianMatrix> & J,
758 template<
typename,
int>
759 class JointCollectionTpl,
760 typename ConfigVector1,
761 typename ConfigVector2,
762 typename JacobianMatrix>
765 const Eigen::MatrixBase<ConfigVector1> & q0,
766 const Eigen::MatrixBase<ConfigVector2> & q1,
767 const Eigen::MatrixBase<JacobianMatrix> & J,
771 LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVector1, ConfigVector2,
773 model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, J), arg);
790 template<
typename,
int>
791 class JointCollectionTpl,
792 typename ConfigVectorIn1,
793 typename ConfigVectorIn2>
795 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
796 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
797 const Eigen::MatrixBase<ConfigVectorIn2> & q1);
814 template<
typename,
int>
815 class JointCollectionTpl,
816 typename ConfigVectorIn1,
817 typename ConfigVectorIn2>
820 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
821 const Eigen::MatrixBase<ConfigVectorIn2> & q1)
824 LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
825 model, q0.derived(), q1.derived());
844 template<
typename,
int>
845 class JointCollectionTpl,
846 typename ConfigVectorIn1,
847 typename ConfigVectorIn2>
849 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
850 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
851 const Eigen::MatrixBase<ConfigVectorIn2> & q1);
867 template<
typename,
int>
868 class JointCollectionTpl,
869 typename ConfigVectorIn1,
870 typename ConfigVectorIn2>
873 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
874 const Eigen::MatrixBase<ConfigVectorIn2> & q1)
877 LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
878 model, q0.derived(), q1.derived());
893 template<
typename,
int>
894 class JointCollectionTpl,
895 typename ConfigVectorType>
897 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
898 const Eigen::MatrixBase<ConfigVectorType> & qout);
911 template<
typename,
int>
912 class JointCollectionTpl,
913 typename ConfigVectorType>
916 const Eigen::MatrixBase<ConfigVectorType> & qout)
918 normalize<LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType>(
919 model, PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, qout));
937 template<
typename,
int>
938 class JointCollectionTpl,
939 typename ConfigVectorType>
941 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
942 const Eigen::MatrixBase<ConfigVectorType> & q,
943 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision());
959 template<
typename,
int>
960 class JointCollectionTpl,
961 typename ConfigVectorType>
964 const Eigen::MatrixBase<ConfigVectorType> & q,
965 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
967 return isNormalized<LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType>(
991 template<
typename,
int>
992 class JointCollectionTpl,
993 typename ConfigVectorIn1,
994 typename ConfigVectorIn2>
996 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
997 const Eigen::MatrixBase<ConfigVectorIn1> & q1,
998 const Eigen::MatrixBase<ConfigVectorIn2> & q2,
999 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision());
1020 template<
typename,
int>
1021 class JointCollectionTpl,
1022 typename ConfigVectorIn1,
1023 typename ConfigVectorIn2>
1026 const Eigen::MatrixBase<ConfigVectorIn1> & q1,
1027 const Eigen::MatrixBase<ConfigVectorIn2> & q2,
1028 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
1031 LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1032 model, q1.derived(), q2.derived(), prec);
1048 typename LieGroup_t,
1051 template<
typename,
int>
1052 class JointCollectionTpl,
1053 typename ConfigVector,
1054 typename JacobianMatrix>
1056 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1057 const Eigen::MatrixBase<ConfigVector> & q,
1058 const Eigen::MatrixBase<JacobianMatrix> & jacobian);
1075 template<
typename,
int>
1076 class JointCollectionTpl,
1077 typename ConfigVector,
1078 typename JacobianMatrix>
1081 const Eigen::MatrixBase<ConfigVector> & q,
1082 const Eigen::MatrixBase<JacobianMatrix> & jacobian)
1085 LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVector, JacobianMatrix>(
1086 model, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, jacobian));
1108 typename LieGroup_t,
1111 template<
typename,
int>
1112 class JointCollectionTpl,
1113 typename ConfigVectorType,
1114 typename TangentVectorType>
1115 typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType)
integrate(
1116 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1117 const Eigen::MatrixBase<ConfigVectorType> & q,
1118 const Eigen::MatrixBase<TangentVectorType> & v);
1136 template<
typename,
int>
1137 class JointCollectionTpl,
1138 typename ConfigVectorType,
1139 typename TangentVectorType>
1140 typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType)
integrate(
1142 const Eigen::MatrixBase<ConfigVectorType> & q,
1143 const Eigen::MatrixBase<TangentVectorType> & v)
1146 LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType>(
1147 model, q.derived(), v.derived());
1164 typename LieGroup_t,
1167 template<
typename,
int>
1168 class JointCollectionTpl,
1169 typename ConfigVectorIn1,
1170 typename ConfigVectorIn2>
1171 typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
interpolate(
1172 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1173 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1174 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
1193 template<
typename,
int>
1194 class JointCollectionTpl,
1195 typename ConfigVectorIn1,
1196 typename ConfigVectorIn2>
1199 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1200 const Eigen::MatrixBase<ConfigVectorIn2> & q1,
1204 LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1205 model, q0.derived(), q1.derived(), u);
1221 typename LieGroup_t,
1224 template<
typename,
int>
1225 class JointCollectionTpl,
1226 typename ConfigVectorIn1,
1227 typename ConfigVectorIn2>
1228 typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1)
difference(
1229 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1230 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1231 const Eigen::MatrixBase<ConfigVectorIn2> & q1);
1248 template<
typename,
int>
1249 class JointCollectionTpl,
1250 typename ConfigVectorIn1,
1251 typename ConfigVectorIn2>
1254 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1255 const Eigen::MatrixBase<ConfigVectorIn2> & q1)
1258 LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1259 model, q0.derived(), q1.derived());
1276 typename LieGroup_t,
1279 template<
typename,
int>
1280 class JointCollectionTpl,
1281 typename ConfigVectorIn1,
1282 typename ConfigVectorIn2>
1284 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1285 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1286 const Eigen::MatrixBase<ConfigVectorIn2> & q1);
1304 template<
typename,
int>
1305 class JointCollectionTpl,
1306 typename ConfigVectorIn1,
1307 typename ConfigVectorIn2>
1310 const Eigen::MatrixBase<ConfigVectorIn1> & q0,
1311 const Eigen::MatrixBase<ConfigVectorIn2> & q1)
1314 LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1315 model, q0.derived(), q1.derived());
1337 typename LieGroup_t,
1340 template<
typename,
int>
1341 class JointCollectionTpl,
1342 typename ConfigVectorIn1,
1343 typename ConfigVectorIn2>
1348 const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
1349 const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits);
1372 template<
typename,
int>
1373 class JointCollectionTpl,
1374 typename ConfigVectorIn1,
1375 typename ConfigVectorIn2>
1380 const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
1381 const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits)
1384 LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
1385 model, lowerLimits.derived(), upperLimits.derived());
1407 typename LieGroup_t,
1410 template<
typename,
int>
1411 class JointCollectionTpl>
1434 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
1439 return randomConfiguration<LieGroupMap, Scalar, Options, JointCollectionTpl>(model);
1453 typename LieGroup_t,
1456 template<
typename,
int>
1457 class JointCollectionTpl>
1458 Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options>
1459 neutral(
const ModelTpl<Scalar, Options, JointCollectionTpl> & model);
1469 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
1470 Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options>
1473 return neutral<LieGroupMap, Scalar, Options, JointCollectionTpl>(model);
1481 #include "pinocchio/algorithm/joint-configuration.hxx"
1483 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
1484 #include "pinocchio/algorithm/joint-configuration.txx"
Main pinocchio namespace.
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
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.
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_EIGEN_PLAIN_TYPE_NO_PARENS((typename ModelTpl< Scalar, Options, JointCollectionTpl >::ConfigVectorType)) randomConfiguration(const ModelTpl< Scalar
Generate a configuration vector uniformly sampled among given limits.
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...
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.
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.
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.
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.
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.
Scalar distance(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1)
Distance between two configuration vectors, namely .
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.
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,...
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
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.
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...
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.