Enumerations | |
enum | ComparisonOperators { LT, LE, EQ, GE, GT } |
Functions | |
PINOCCHIO_EXTRA_DLLAPI void | buildConvexHull (ReachableSetResults &res) |
Computes the convex hull using qhull associated with the vertex stored in res. More... | |
template<typename LhsType , typename RhsType > | |
bool | comparison_eq (const LhsType &lhs_value, const RhsType &rhs_value) |
template<typename Scalar , typename ConstraintAllocator , typename VectorLikeIn , typename VectorLikeOut > | |
void | computeComplementarityShift (const std::vector< CoulombFrictionConeTpl< Scalar >, ConstraintAllocator > &cones, const Eigen::DenseBase< VectorLikeIn > &velocities, const Eigen::DenseBase< VectorLikeOut > &shift_) |
template<typename Scalar , typename ConstraintAllocator , typename VectorLikeIn , typename VectorLikeOut > | |
void | computeConeProjection (const std::vector< CoulombFrictionConeTpl< Scalar >, ConstraintAllocator > &cones, const Eigen::DenseBase< VectorLikeIn > &x, const Eigen::DenseBase< VectorLikeOut > &x_proj_) |
Project a vector x on the vector of cones. More... | |
template<typename Scalar , typename ConstraintAllocator , typename VectorLikeVelocity , typename VectorLikeForce > | |
Scalar | computeConicComplementarity (const std::vector< CoulombFrictionConeTpl< Scalar >, ConstraintAllocator > &cones, const Eigen::DenseBase< VectorLikeVelocity > &velocities, const Eigen::DenseBase< VectorLikeForce > &forces) |
template<typename Scalar , typename ConstraintAllocator , typename VectorLikeIn , typename VectorLikeOut > | |
void | computeDualConeProjection (const std::vector< CoulombFrictionConeTpl< Scalar >, ConstraintAllocator > &cones, const Eigen::DenseBase< VectorLikeIn > &x, const Eigen::DenseBase< VectorLikeOut > &x_proj_) |
Project a vector x on the dual of the cones contained in the vector of cones. More... | |
void | computeJointVel (const Eigen::VectorXd &res1, const Eigen::VectorXd &res2, const Eigen::VectorXi &comb, Eigen::VectorXd &qv) |
Computes the joint configuration associated with the permutation results stored in res1 and res2. More... | |
template<typename Scalar , typename ConstraintAllocator , typename VectorLikeIn > | |
Scalar | computePrimalFeasibility (const std::vector< CoulombFrictionConeTpl< Scalar >, ConstraintAllocator > &cones, const Eigen::DenseBase< VectorLikeIn > &forces) |
template<typename Scalar , typename ConstraintAllocator , typename ForceVector , typename VelocityVector > | |
Scalar | computeReprojectionError (const std::vector< CoulombFrictionConeTpl< Scalar >, ConstraintAllocator > &cones, const Eigen::DenseBase< ForceVector > &forces, const Eigen::DenseBase< VelocityVector > &velocities) |
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , class FilterFunction > | |
void | computeVertex (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q0, const double time_horizon, const int frame_id, FilterFunction config_filter, Eigen::MatrixXd &vertex, const ReachableSetParams ¶ms=ReachableSetParams()) |
Samples points to create the reachable workspace that will respect mechanical limits of the model as well as the time horizon. More... | |
void | generateCombination (const int n, const int k, Eigen::VectorXi &indices) |
Return a subsequence of length k of elements from range 0 to n. Inspired by https://docs.python.org/3/library/itertools.html#itertools.combinations. Indices table will hold the results. More... | |
template<typename LhsType , typename RhsType , typename ThenType , typename ElseType > | |
if_then_else_impl< LhsType, RhsType, ThenType, ElseType >::ReturnType | if_then_else (const ComparisonOperators op, const LhsType &lhs_value, const RhsType &rhs_value, const ThenType &then_value, const ElseType &else_value) |
void | productCombination (const Eigen::VectorXd &element, const int repeat, Eigen::VectorXi &indices, Eigen::VectorXd &combination) |
Cartesian product of input element with itself. Number of repetition is specified with repeat argument. Inspired by https://docs.python.org/3/library/itertools.html#itertools.product. More... | |
Enumerator | |
---|---|
LT | |
LE | |
EQ | |
GE | |
GT |
Definition at line 15 of file utils/static-if.hpp.
void pinocchio::internal::buildConvexHull | ( | ReachableSetResults & | res | ) |
Computes the convex hull using qhull associated with the vertex stored in res.
[out] | res | Contain both the points and the faces of the convex hull |
Definition at line 17 of file src/extra/reachable-workspace.cpp.
|
inline |
Definition at line 120 of file utils/static-if.hpp.
void pinocchio::internal::computeComplementarityShift | ( | const std::vector< CoulombFrictionConeTpl< Scalar >, ConstraintAllocator > & | cones, |
const Eigen::DenseBase< VectorLikeIn > & | velocities, | ||
const Eigen::DenseBase< VectorLikeOut > & | shift_ | ||
) |
Definition at line 92 of file contact-solver-utils.hpp.
void pinocchio::internal::computeConeProjection | ( | const std::vector< CoulombFrictionConeTpl< Scalar >, ConstraintAllocator > & | cones, |
const Eigen::DenseBase< VectorLikeIn > & | x, | ||
const Eigen::DenseBase< VectorLikeOut > & | x_proj_ | ||
) |
Project a vector x on the vector of cones.
Definition at line 25 of file contact-solver-utils.hpp.
Scalar pinocchio::internal::computeConicComplementarity | ( | const std::vector< CoulombFrictionConeTpl< Scalar >, ConstraintAllocator > & | cones, |
const Eigen::DenseBase< VectorLikeVelocity > & | velocities, | ||
const Eigen::DenseBase< VectorLikeForce > & | forces | ||
) |
Definition at line 68 of file contact-solver-utils.hpp.
void pinocchio::internal::computeDualConeProjection | ( | const std::vector< CoulombFrictionConeTpl< Scalar >, ConstraintAllocator > & | cones, |
const Eigen::DenseBase< VectorLikeIn > & | x, | ||
const Eigen::DenseBase< VectorLikeOut > & | x_proj_ | ||
) |
Project a vector x on the dual of the cones contained in the vector of cones.
Definition at line 47 of file contact-solver-utils.hpp.
void pinocchio::internal::computeJointVel | ( | const Eigen::VectorXd & | res1, |
const Eigen::VectorXd & | res2, | ||
const Eigen::VectorXi & | comb, | ||
Eigen::VectorXd & | qv | ||
) |
Computes the joint configuration associated with the permutation results stored in res1 and res2.
[in] | res1 | First permutation result |
[in] | res2 | Second permutation result |
[in] | comb | Vector of active joints in this configuration |
[out] | qv | Joint Velocity result |
Scalar pinocchio::internal::computePrimalFeasibility | ( | const std::vector< CoulombFrictionConeTpl< Scalar >, ConstraintAllocator > & | cones, |
const Eigen::DenseBase< VectorLikeIn > & | forces | ||
) |
Definition at line 109 of file contact-solver-utils.hpp.
Scalar pinocchio::internal::computeReprojectionError | ( | const std::vector< CoulombFrictionConeTpl< Scalar >, ConstraintAllocator > & | cones, |
const Eigen::DenseBase< ForceVector > & | forces, | ||
const Eigen::DenseBase< VelocityVector > & | velocities | ||
) |
Definition at line 134 of file contact-solver-utils.hpp.
void pinocchio::internal::computeVertex | ( | const ModelTpl< Scalar, Options, JointCollectionTpl > & | model, |
const Eigen::MatrixBase< ConfigVectorType > & | q0, | ||
const double | time_horizon, | ||
const int | frame_id, | ||
FilterFunction | config_filter, | ||
Eigen::MatrixXd & | vertex, | ||
const ReachableSetParams & | params = ReachableSetParams() |
||
) |
Samples points to create the reachable workspace that will respect mechanical limits of the model as well as the time horizon.
JointCollection | Collection of Joint types. |
ConfigVectorType | Type of the joint configuration vector. |
FilterFunction | Function template use to filter points in the workspace. Prototype : f(model, data) -> bool |
[in] | model | The model structure of the rigid body system. |
[in] | geom_model | Geometry model associated with the model |
[in] | q | The initial joint configuration vector (dim model.nq). |
[in] | time_horizon | time horizon for which the polytope will be computed (in seconds) |
[in] | frame_id | Index of the frame for which the workspace should be computed. |
[in] | params | parameters of the algorithm |
[out] | vertex | Results of algorithm |
void pinocchio::internal::generateCombination | ( | const int | n, |
const int | k, | ||
Eigen::VectorXi & | indices | ||
) |
Return a subsequence of length k of elements from range 0 to n. Inspired by https://docs.python.org/3/library/itertools.html#itertools.combinations. Indices table will hold the results.
[in] | n | Max range of element |
[in] | k | length of subsequences |
[out] | indices | results of the combination |
|
inline |
Definition at line 89 of file utils/static-if.hpp.
void pinocchio::internal::productCombination | ( | const Eigen::VectorXd & | element, |
const int | repeat, | ||
Eigen::VectorXi & | indices, | ||
Eigen::VectorXd & | combination | ||
) |
Cartesian product of input element with itself. Number of repetition is specified with repeat argument. Inspired by https://docs.python.org/3/library/itertools.html#itertools.product.
[in] | element | Vector for which the cartesian product is needed. |
[in] | repeat | Number of repetition |
[in] | indices | Vector of indexes of which element will be repeated (will be changed during function call) |
[out] | combination | Cartesian Product associated with the indices |