hpp-manipulation  4.11.0
Classes for manipulation planning.
helper.hh
Go to the documentation of this file.
1 // Copyright (c) 2016, LAAS-CNRS
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4 // This file is part of hpp-manipulation.
5 // hpp-manipulation is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // hpp-manipulation is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
16 
17 #ifndef HPP_MANIPULATION_GRAPH_HELPER_HH
18 # define HPP_MANIPULATION_GRAPH_HELPER_HH
19 
20 # include <string>
21 # include <algorithm>
22 
23 # include <tuple>
24 
26 # include "hpp/manipulation/fwd.hh"
28 
29 namespace hpp {
30  namespace manipulation {
32  namespace graph {
33  namespace helper {
37 
41  nc.reserve (a.size() + b.size());
42  std::copy (a.begin(), a.end(), nc.begin());
43  std::copy (b.begin(), b.end(), nc.begin());
44  return nc;
45  }
46 
48  // Manifold definition
52  // Foliation definition
55 
57  FoliatedManifold both;
58  both.nc = merge_nc(nc, other.nc);
59  both.nc_path = merge_nc(nc_path, other.nc_path);
60 
61  std::copy (lj.begin (), lj.end (), both.lj.end ());
62  std::copy (other.lj.begin (), other.lj.end (), both.lj.end ());
63  return both;
64  }
65 
66  void addToState (StatePtr_t comp) const;
67  void addToEdge (EdgePtr_t comp) const;
68  void specifyFoliation (LevelSetEdgePtr_t lse) const;
69 
70  bool foliated () const {
71  return !lj_fol.empty () || !nc_fol.empty ();
72  }
73  bool empty () const {
74  return lj.empty () && nc.empty ();
75  }
76  };
77 
78  enum GraspingCase {
79  NoGrasp = 1 << 0,
80  GraspOnly = 1 << 1,
81  WithPreGrasp = 1 << 2
82  };
84  NoPlace = 1 << 3,
85  PlaceOnly = 1 << 4,
86  WithPrePlace = 1 << 5
87  };
88 
89  struct Rule {
90  std::vector<std::string> grippers_;
91  std::vector<std::string> handles_;
92  bool link_;
93  Rule() : grippers_(), handles_(), link_(false) {}
94  };
95 
96  typedef std::vector<Rule> Rules_t;
97 
103  template < int gCase >
105  const std::string& forwName, const std::string& backName,
106  const StatePtr_t& from, const StatePtr_t& to,
107  const size_type& wForw, const size_type& wBack,
108  const FoliatedManifold& grasp, const FoliatedManifold& pregrasp,
109  const FoliatedManifold& place, const FoliatedManifold& preplace,
110  const bool levelSetGrasp, const bool levelSetPlace,
111  const FoliatedManifold& submanifoldDef = FoliatedManifold ()
112  );
113 
115  const std::string& loopName,
116  const StatePtr_t& state,
117  const size_type& w,
118  const bool levelSet,
119  const FoliatedManifold& submanifoldDef = FoliatedManifold ()
120  );
121 
126 
131 
132  void graspManifold (
133  const GripperPtr_t& gripper, const HandlePtr_t& handle,
134  FoliatedManifold& grasp, FoliatedManifold& pregrasp);
135 
139  const ImplicitPtr_t placement,
140  const ImplicitPtr_t preplacement,
141  const ImplicitPtr_t placementComplement,
142  FoliatedManifold& place, FoliatedManifold& preplace);
143 
148  const ImplicitPtr_t placement,
149  const ImplicitPtr_t preplacement,
150  const LockedJoints_t objectLocks,
151  FoliatedManifold& place, FoliatedManifold& preplace);
152 
153  typedef std::tuple <ImplicitPtr_t,
157  typedef std::vector <HandlePtr_t> Handles_t;
158  typedef std::vector <GripperPtr_t> Grippers_t;
164  typedef std::tuple <PlacementConstraint_t, Handles_t, std::size_t> Object_t;
165  typedef std::vector <Object_t> Objects_t;
166 
173  void graphBuilder (
174  const ProblemSolverPtr_t& ps,
175  const Objects_t& objects,
176  const Grippers_t& grippers,
177  GraphPtr_t graph,
178  const Rules_t& rules = Rules_t ());
179 
180  struct ObjectDef_t {
181  std::string name;
183  };
184 
186  const ProblemSolverPtr_t& ps,
187  const std::string& graphName,
188  const StringList_t& griNames,
189  const std::list <ObjectDef_t>& objs,
190  const StringList_t& envNames,
191  const Rules_t& rules,
192  const value_type& prePlaceWidth = 0.05);
194  } // namespace helper
195  } // namespace graph
196  } // namespace manipulation
197 } // namespace hpp
198 
199 #endif // HPP_MANIPULATION_GRAPH_HELPER_HH
NumericalConstraints_t nc_path
Definition: helper.hh:51
std::vector< std::string > handles_
Definition: helper.hh:91
std::vector< EdgePtr_t > Edges_t
Definition: fwd.hh:46
std::tuple< PlacementConstraint_t, Handles_t, std::size_t > Object_t
Definition: helper.hh:164
std::string name
Definition: helper.hh:181
void strictPlacementManifold(const ImplicitPtr_t placement, const ImplicitPtr_t preplacement, const ImplicitPtr_t placementComplement, FoliatedManifold &place, FoliatedManifold &preplace)
void graspManifold(const GripperPtr_t &gripper, const HandlePtr_t &handle, FoliatedManifold &grasp, FoliatedManifold &pregrasp)
EdgePtr_t createLoopEdge(const std::string &loopName, const StatePtr_t &state, const size_type &w, const bool levelSet, const FoliatedManifold &submanifoldDef=FoliatedManifold())
Definition: main.hh:1
LockedJoints_t lj
Definition: helper.hh:50
std::vector< HandlePtr_t > Handles_t
Definition: helper.hh:157
std::tuple< ImplicitPtr_t, ImplicitPtr_t, LockedJoints_t > PlacementConstraint_t
Definition: helper.hh:156
core::value_type value_type
Definition: fwd.hh:82
NumericalConstraints_t merge_nc(const NumericalConstraints_t &a, const NumericalConstraints_t &b)
Definition: helper.hh:39
bool empty() const
Definition: helper.hh:73
std::vector< GripperPtr_t > Grippers_t
Definition: helper.hh:158
std::vector< Object_t > Objects_t
Definition: helper.hh:165
bool foliated() const
Definition: helper.hh:70
void specifyFoliation(LevelSetEdgePtr_t lse) const
shared_ptr< LevelSetEdge > LevelSetEdgePtr_t
Definition: fwd.hh:39
std::vector< Rule > Rules_t
Definition: helper.hh:96
shared_ptr< Handle > HandlePtr_t
Definition: fwd.hh:48
LockedJoints_t lj_fol
Definition: helper.hh:54
hpp::core::LockedJoints_t LockedJoints_t
Definition: fwd.hh:54
shared_ptr< Graph > GraphPtr_t
Definition: fwd.hh:35
PlacementCase
Definition: helper.hh:83
constraints::ImplicitPtr_t ImplicitPtr_t
Definition: fwd.hh:108
Rule()
Definition: helper.hh:93
hpp::core::NumericalConstraints_t NumericalConstraints_t
Definition: fwd.hh:53
shared_ptr< State > StatePtr_t
Definition: fwd.hh:36
StringList_t shapes
Definition: helper.hh:182
GraspingCase
Definition: helper.hh:78
std::list< std::string > StringList_t
Definition: fwd.hh:135
NumericalConstraints_t nc
Definition: helper.hh:49
core::size_type size_type
Definition: fwd.hh:83
shared_ptr< Edge > EdgePtr_t
Definition: fwd.hh:37
void graphBuilder(const ProblemSolverPtr_t &ps, const Objects_t &objects, const Grippers_t &grippers, GraphPtr_t graph, const Rules_t &rules=Rules_t())
FoliatedManifold merge(const FoliatedManifold &other)
Definition: helper.hh:56
void relaxedPlacementManifold(const ImplicitPtr_t placement, const ImplicitPtr_t preplacement, const LockedJoints_t objectLocks, FoliatedManifold &place, FoliatedManifold &preplace)
Edges_t createEdges(const std::string &forwName, const std::string &backName, const StatePtr_t &from, const StatePtr_t &to, const size_type &wForw, const size_type &wBack, const FoliatedManifold &grasp, const FoliatedManifold &pregrasp, const FoliatedManifold &place, const FoliatedManifold &preplace, const bool levelSetGrasp, const bool levelSetPlace, const FoliatedManifold &submanifoldDef=FoliatedManifold())
std::vector< std::string > grippers_
Definition: helper.hh:90
NumericalConstraints_t nc_fol
Definition: helper.hh:53
Definition: problem-solver.hh:33
pinocchio::GripperPtr_t GripperPtr_t
Definition: fwd.hh:41
bool link_
Definition: helper.hh:92