pinocchio  3.2.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>
62  class JointCollectionTpl,
63  typename ConfigVectorType>
64  void reachableWorkspace(
65  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
66  const Eigen::MatrixBase<ConfigVectorType> & q0,
67  const double time_horizon,
68  const int frame_id,
69  Eigen::MatrixXd & vertex,
70  const ReachableSetParams & params = ReachableSetParams());
71 
77 
83 
85  template<
86  typename Scalar,
87  int Options,
88  template<typename, int>
89  class JointCollectionTpl,
90  typename ConfigVectorType>
91  void reachableWorkspaceHull(
92  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
93  const Eigen::MatrixBase<ConfigVectorType> & q0,
94  const double time_horizon,
95  const int frame_id,
96  ReachableSetResults & res,
97  const ReachableSetParams & params = ReachableSetParams());
98 
99 #ifdef PINOCCHIO_WITH_HPP_FCL
106 
112 
114  template<
115  typename Scalar,
116  int Options,
117  template<typename, int>
118  class JointCollectionTpl,
119  typename ConfigVectorType>
120  void reachableWorkspaceWithCollisions(
121  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
122  const GeometryModel & geom_model,
123  const Eigen::MatrixBase<ConfigVectorType> & q0,
124  const double time_horizon,
125  const int frame_id,
126  Eigen::MatrixXd & vertex,
127  const ReachableSetParams & params = ReachableSetParams());
128 
136 
143 
145  template<
146  typename Scalar,
147  int Options,
148  template<typename, int>
149  class JointCollectionTpl,
150  typename ConfigVectorType>
151  void reachableWorkspaceWithCollisionsHull(
152  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
153  const GeometryModel & geom_model,
154  const Eigen::MatrixBase<ConfigVectorType> & q0,
155  const double time_horizon,
156  const int frame_id,
157  ReachableSetResults & res,
158  const ReachableSetParams & params = ReachableSetParams());
159 #endif // PINOCCHIO_WITH_HPP_FCL
160 
161  namespace internal
162  {
177 
179  template<
180  typename Scalar,
181  int Options,
182  template<typename, int>
183  class JointCollectionTpl,
184  typename ConfigVectorType,
185  class FilterFunction>
186  void computeVertex(
187  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
188  const Eigen::MatrixBase<ConfigVectorType> & q0,
189  const double time_horizon,
190  const int frame_id,
191  FilterFunction config_filter,
192  Eigen::MatrixXd & vertex,
193  const ReachableSetParams & params = ReachableSetParams());
194 
197  PINOCCHIO_EXTRA_DLLAPI void buildConvexHull(ReachableSetResults & res);
198 
202 
204  void computeJointVel(
205  const Eigen::VectorXd & res1,
206  const Eigen::VectorXd & res2,
207  const Eigen::VectorXi & comb,
208  Eigen::VectorXd & qv);
209 
213 
215  void generateCombination(const int n, const int k, Eigen::VectorXi & indices);
216 
223 
225  void productCombination(
226  const Eigen::VectorXd & element,
227  const int repeat,
228  Eigen::VectorXi & indices,
229  Eigen::VectorXd & combination);
230  } // namespace internal
231 } // namespace pinocchio
232 
233 /* --- Details -------------------------------------------------------------------- */
234 #include "pinocchio/extra/reachable-workspace.hxx"
235 
236 #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