pinocchio  3.6.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
reachable-workspace.hpp
1 //
2 // Copyright (c) 2016-2023 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_extra_reachable_workspace_hpp__
6 #define __pinocchio_extra_reachable_workspace_hpp__
7 
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"
13 
14 #ifdef PINOCCHIO_WITH_HPP_FCL
15  #include "pinocchio/collision/collision.hpp"
16 #endif // PINOCCHIO_WITH_HPP_FCL
17 
18 #include <Eigen/Core>
19 
20 namespace pinocchio
21 {
25  struct ReachableSetResults
26  {
27  Eigen::MatrixXd vertex;
28  Eigen::MatrixXi faces;
29  };
30 
39  struct ReachableSetParams
40  {
41  int n_samples = 5;
42  int facet_dims = 3;
43  };
44 
50 
56 
58  template<
59  typename Scalar,
60  int Options,
61  template<typename, int> class JointCollectionTpl,
62  typename ConfigVectorType>
63  void reachableWorkspace(
64  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
65  const Eigen::MatrixBase<ConfigVectorType> & q0,
66  const double time_horizon,
67  const int frame_id,
68  Eigen::MatrixXd & vertex,
69  const ReachableSetParams & params = ReachableSetParams());
70 
76 
82 
84  template<
85  typename Scalar,
86  int Options,
87  template<typename, int> class JointCollectionTpl,
88  typename ConfigVectorType>
89  void reachableWorkspaceHull(
90  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
91  const Eigen::MatrixBase<ConfigVectorType> & q0,
92  const double time_horizon,
93  const int frame_id,
94  ReachableSetResults & res,
95  const ReachableSetParams & params = ReachableSetParams());
96 
97 #ifdef PINOCCHIO_WITH_HPP_FCL
104 
110 
112  template<
113  typename Scalar,
114  int Options,
115  template<typename, int> class JointCollectionTpl,
116  typename ConfigVectorType>
117  void reachableWorkspaceWithCollisions(
118  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
119  const GeometryModel & geom_model,
120  const Eigen::MatrixBase<ConfigVectorType> & q0,
121  const double time_horizon,
122  const int frame_id,
123  Eigen::MatrixXd & vertex,
124  const ReachableSetParams & params = ReachableSetParams());
125 
133 
140 
142  template<
143  typename Scalar,
144  int Options,
145  template<typename, int> class JointCollectionTpl,
146  typename ConfigVectorType>
147  void reachableWorkspaceWithCollisionsHull(
148  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
149  const GeometryModel & geom_model,
150  const Eigen::MatrixBase<ConfigVectorType> & q0,
151  const double time_horizon,
152  const int frame_id,
153  ReachableSetResults & res,
154  const ReachableSetParams & params = ReachableSetParams());
155 #endif // PINOCCHIO_WITH_HPP_FCL
156 
157  namespace internal
158  {
173 
175  template<
176  typename Scalar,
177  int Options,
178  template<typename, int> class JointCollectionTpl,
179  typename ConfigVectorType,
180  class FilterFunction>
181  void computeVertex(
182  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
183  const Eigen::MatrixBase<ConfigVectorType> & q0,
184  const double time_horizon,
185  const int frame_id,
186  FilterFunction config_filter,
187  Eigen::MatrixXd & vertex,
188  const ReachableSetParams & params = ReachableSetParams());
189 
192  PINOCCHIO_EXTRA_DLLAPI void buildConvexHull(ReachableSetResults & res);
193 
197 
199  void computeJointVel(
200  const Eigen::VectorXd & res1,
201  const Eigen::VectorXd & res2,
202  const Eigen::VectorXi & comb,
203  Eigen::VectorXd & qv);
204 
208 
210  void generateCombination(const int n, const int k, Eigen::VectorXi & indices);
211 
218 
220  void productCombination(
221  const Eigen::VectorXd & element,
222  const int repeat,
223  Eigen::VectorXi & indices,
224  Eigen::VectorXd & combination);
225  } // namespace internal
226 } // namespace pinocchio
227 
228 /* --- Details -------------------------------------------------------------------- */
229 #include "pinocchio/extra/reachable-workspace.hxx"
230 
231 #endif // ifndef __pinocchio_extra_reachable_workspace_hpp__
pinocchio::internal::computeJointVel
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.
pinocchio::internal::productCombination
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...
pinocchio::internal::generateCombination
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::internal::buildConvexHull
PINOCCHIO_EXTRA_DLLAPI void buildConvexHull(ReachableSetResults &res)
Computes the convex hull using qhull associated with the vertex stored in res.
pinocchio::internal::computeVertex
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 &params=ReachableSetParams())
Samples points to create the reachable workspace that will respect mechanical limits of the model as ...
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::reachableWorkspaceWithCollisions
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 &params=ReachableSetParams())
Computes the reachable workspace with respect to a geometry model on a fixed time horizon....
pinocchio::reachableWorkspaceWithCollisionsHull
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 &params=ReachableSetParams())
Computes the convex Hull of the reachable workspace with respect to a geometry model on a fixed time ...
pinocchio::reachableWorkspace
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 &params=ReachableSetParams())
Computes the reachable workspace on a fixed time horizon. For more information, please see https://gi...
pinocchio::reachableWorkspaceHull
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 &params=ReachableSetParams())
Computes the convex Hull reachable workspace on a fixed time horizon. For more information,...
pinocchio::ReachableSetParams
Parameters for the reachable space algorithm.
Definition: reachable-workspace.hpp:40
pinocchio::ReachableSetResults
Structure containing the return value for the reachable algorithm.
Definition: reachable-workspace.hpp:26