hpp-core  4.12.0
Implement basic classes for canonical path planning for kinematic chains.
global.hh
Go to the documentation of this file.
1 
2 // Copyright (c) 2014, LAAS-CNRS
3 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
4 //
5 // This file is part of hpp-core.
6 // hpp-core is free software: you can redistribute it
7 // and/or modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation, either version
9 // 3 of the License, or (at your option) any later version.
10 //
11 // hpp-core is distributed in the hope that it will be
12 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Lesser Public License for more details. You should have
15 // received a copy of the GNU Lesser General Public License along with
16 // hpp-core. If not, see <http://www.gnu.org/licenses/>.
17 
18 #ifndef HPP_CORE_PATHPROJECTOR_GLOBAL_HH
19 # define HPP_CORE_PATHPROJECTOR_GLOBAL_HH
20 
21 // lineSearch::FixedSequence
22 # include <hpp/constraints/solver/hierarchical-iterative.hh>
23 
26 
27 namespace hpp {
28  namespace core {
29  namespace pathProjector {
31  {
32  public:
35 
41  static GlobalPtr_t create (const DistancePtr_t& distance,
42  const SteeringMethodPtr_t& steeringMethod, value_type step);
43 
44  static GlobalPtr_t create (const ProblemConstPtr_t& problem,
45  const value_type& step);
46 
47  protected:
48  bool impl_apply (const PathPtr_t& path,
49  PathPtr_t& projection) const;
50 
51  Global (const DistancePtr_t& distance,
52  const SteeringMethodPtr_t& steeringMethod,
53  value_type step, value_type threshold, value_type hessianBound);
54  private:
55  value_type step_;
56 
57  const value_type hessianBound_;
58  const value_type thresholdMin_;
59 
60  typedef constraints::solver::lineSearch::FixedSequence LineSearch_t;
61  struct Data {
63  value_type length; // Length between this config and the previous one
64  LineSearch_t alpha;
65  std::size_t Niter;
66  value_type sigma;
67  bool projected;
68  };
69 
70  typedef std::list <Configuration_t,
71  Eigen::aligned_allocator <Configuration_t> > Configs_t;
72  typedef std::list <value_type> Lengths_t;
73  typedef std::list <LineSearch_t> Alphas_t;
74  typedef std::vector <bool> Bools_t;
75  typedef std::list<Data> Datas_t;
76 
77  bool projectOneStep (ConfigProjector& p,
78  Configs_t& q, Configs_t::iterator& last,
79  Bools_t& b, Lengths_t& l, Alphas_t& alpha) const;
80 
81  bool projectOneStep (ConfigProjector& p,
82  Datas_t& ds, const Datas_t::iterator& last) const;
83 
85  size_type reinterpolate (const DevicePtr_t& robot,
86  Configs_t& q, const Configs_t::iterator& last,
87  Bools_t& b, Lengths_t& l, Alphas_t& alpha,
88  const value_type& maxDist) const;
89 
91  size_type reinterpolate (const DevicePtr_t& robot,
92  ConfigProjector& p, Datas_t& q, Datas_t::iterator& last) const;
93 
94  bool createPath (const DevicePtr_t& robot,
95  const ConstraintSetPtr_t& constraint,
96  const Configs_t& q, const Configs_t::iterator& last,
97  const Bools_t& b, const Lengths_t& l,
98  PathPtr_t& result) const;
99 
100  bool createPath (const DevicePtr_t& robot,
101  const ConstraintSetPtr_t& constraint,
102  const Datas_t& ds, const Datas_t::iterator& last,
103  PathPtr_t& result) const;
104 
105  bool project (const PathPtr_t& path, PathPtr_t& projection) const;
106 
107  bool project2 (const PathPtr_t& path, PathPtr_t& projection) const;
108 
109  void initialConfigList (const PathPtr_t& path,
110  Configs_t& cfgs) const;
111 
112  void initialConfigList (const PathPtr_t& path,
113  ConfigProjector& p, Datas_t& cfgs) const;
114 
115  void initData (Data& data, const Configuration_t& q,
116  ConfigProjector& p, bool computeSigma = false,
117  bool projected = false,
118  const Configuration_t& distTo = Configuration_t()) const;
119 
120  mutable Configuration_t q_;
121  mutable vector_t dq_;
122  };
123  } // namespace pathProjector
124  } // namespace core
125 } // namespace hpp
126 
127 #endif // HPP_CORE_PATHPROJECTOR_GLOBAL_HH
hpp::core::PathPtr_t PathPtr_t
Definition: path-projector.hh:30
shared_ptr< Global > GlobalPtr_t
Definition: fwd.hh:310
hpp::core::StraightPath StraightPath
Definition: global.hh:33
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:114
Definition: bi-rrt-planner.hh:24
pinocchio::size_type size_type
Definition: fwd.hh:156
shared_ptr< const Problem > ProblemConstPtr_t
Definition: fwd.hh:180
shared_ptr< StraightPath > StraightPathPtr_t
Definition: fwd.hh:183
This class projects a path using constraints.
Definition: path-projector.hh:26
shared_ptr< Distance > DistancePtr_t
Definition: fwd.hh:122
Definition: config-projector.hh:57
hpp::core::StraightPathPtr_t StraightPathPtr_t
Definition: global.hh:34
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:110
Definition: global.hh:30
pinocchio::vector_t vector_t
Definition: fwd.hh:202
pinocchio::value_type value_type
Definition: fwd.hh:157
Definition: straight-path.hh:42
shared_ptr< SteeringMethod > SteeringMethodPtr_t
Definition: fwd.hh:195
#define HPP_CORE_DLLAPI
Definition: config.hh:64
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:96