5 #ifndef __pinocchio_extra_reachable_workspace_hpp__
6 #define __pinocchio_extra_reachable_workspace_hpp__
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/multibody/data.hpp"
10 #include "pinocchio/multibody/geometry.hpp"
11 #include "pinocchio/algorithm/geometry.hpp"
12 #include "pinocchio/extra/config.hpp"
14 #ifdef PINOCCHIO_WITH_HPP_FCL
15 #include "pinocchio/collision/collision.hpp"
27 Eigen::MatrixXd vertex;
28 Eigen::MatrixXi faces;
61 template<
typename,
int>
62 class JointCollectionTpl,
63 typename ConfigVectorType>
66 const Eigen::MatrixBase<ConfigVectorType> & q0,
67 const double time_horizon,
69 Eigen::MatrixXd & vertex,
88 template<
typename,
int>
89 class JointCollectionTpl,
90 typename ConfigVectorType>
93 const Eigen::MatrixBase<ConfigVectorType> & q0,
94 const double time_horizon,
99 #ifdef PINOCCHIO_WITH_HPP_FCL
117 template<
typename,
int>
118 class JointCollectionTpl,
119 typename ConfigVectorType>
123 const Eigen::MatrixBase<ConfigVectorType> & q0,
124 const double time_horizon,
126 Eigen::MatrixXd & vertex,
148 template<
typename,
int>
149 class JointCollectionTpl,
150 typename ConfigVectorType>
154 const Eigen::MatrixBase<ConfigVectorType> & q0,
155 const double time_horizon,
182 template<
typename,
int>
183 class JointCollectionTpl,
184 typename ConfigVectorType,
185 class FilterFunction>
188 const Eigen::MatrixBase<ConfigVectorType> & q0,
189 const double time_horizon,
191 FilterFunction config_filter,
192 Eigen::MatrixXd & vertex,
205 const Eigen::VectorXd & res1,
206 const Eigen::VectorXd & res2,
207 const Eigen::VectorXi & comb,
208 Eigen::VectorXd & qv);
226 const Eigen::VectorXd & element,
228 Eigen::VectorXi & indices,
229 Eigen::VectorXd & combination);
234 #include "pinocchio/extra/reachable-workspace.hxx"
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.
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 argumen...
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....
PINOCCHIO_EXTRA_DLLAPI void buildConvexHull(ReachableSetResults &res)
Computes the convex hull using qhull associated with the vertex stored in res.
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 ...
Main pinocchio namespace.
void reachableWorkspaceWithCollisions(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const GeometryModel &geom_model, const Eigen::MatrixBase< ConfigVectorType > &q0, const double time_horizon, const int frame_id, Eigen::MatrixXd &vertex, const ReachableSetParams ¶ms=ReachableSetParams())
Computes the reachable workspace with respect to a geometry model on a fixed time horizon....
void reachableWorkspaceWithCollisionsHull(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const GeometryModel &geom_model, const Eigen::MatrixBase< ConfigVectorType > &q0, const double time_horizon, const int frame_id, ReachableSetResults &res, const ReachableSetParams ¶ms=ReachableSetParams())
Computes the convex Hull of the reachable workspace with respect to a geometry model on a fixed time ...
void reachableWorkspace(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q0, const double time_horizon, const int frame_id, Eigen::MatrixXd &vertex, const ReachableSetParams ¶ms=ReachableSetParams())
Computes the reachable workspace on a fixed time horizon. For more information, please see https://gi...
void reachableWorkspaceHull(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q0, const double time_horizon, const int frame_id, ReachableSetResults &res, const ReachableSetParams ¶ms=ReachableSetParams())
Computes the convex Hull reachable workspace on a fixed time horizon. For more information,...
Parameters for the reachable space algorithm.
Structure containing the return value for the reachable algorithm.