hpp-core  4.12.0
Implement basic classes for canonical path planning for kinematic chains.
constant-curvature.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2017 CNRS
3 // Authors: Florent Lamiraux
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
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef HPP_CORE_STEERING_METHOD_CONSTANT_CURVATURE_HH
20 # define HPP_CORE_STEERING_METHOD_CONSTANT_CURVATURE_HH
21 
22 # include <hpp/core/fwd.hh>
23 # include <hpp/core/path.hh>
25 
26 namespace hpp {
27  namespace core {
28  namespace steeringMethod {
30  class ConstantCurvature : public Path {
31  public:
32  typedef Path parent_t;
33  virtual ~ConstantCurvature () {}
50  (const DevicePtr_t& robot, ConfigurationIn_t init,
52  value_type curveLength, value_type pathLength,
53  value_type curvature, size_type xyId, size_type rzId,
54  const JointPtr_t rz, const std::vector<JointPtr_t> wheels,
56 
59  (const ConstantCurvaturePtr_t& other);
60 
63  (const ConstantCurvaturePtr_t& other,
64  const ConstraintSetPtr_t& constraints);
65 
67  virtual PathPtr_t copy () const;
68 
72  virtual PathPtr_t copy (const ConstraintSetPtr_t& constraints) const
73  {
74  return createCopy (weak_.lock (), constraints);
75  }
76 
78  inline Configuration_t initial () const
79  {
80  return initial_;
81  }
82 
84  inline Configuration_t end () const
85  {
86  return end_;
87  }
88 
89  virtual PathPtr_t reverse () const;
90 
91  struct Wheels_t {
92  value_type value; // Constant value of the wheel angle
94  Wheels_t () : j () {}
95  };
96 
97  protected:
99  virtual std::ostream& print (std::ostream &os) const;
100 
115  ConstantCurvature (const DevicePtr_t& robot, ConfigurationIn_t init,
116  ConfigurationIn_t end,
117  value_type curveLength, value_type pathLength,
118  value_type curvature, size_type xyId,
119  size_type rzId, const JointPtr_t rz,
120  const std::vector<JointPtr_t> wheels);
121 
137  ConstantCurvature (const DevicePtr_t& robot, ConfigurationIn_t init,
138  ConfigurationIn_t end,
139  value_type curveLength, value_type pathLength,
140  value_type curvature, size_type xyId, size_type rzId,
141  const JointPtr_t rz,
142  const std::vector<JointPtr_t> wheels,
143  ConstraintSetPtr_t constraints);
144 
146  ConstantCurvature (const ConstantCurvature& other);
147 
149  ConstantCurvature (const ConstantCurvature& other,
150  const ConstraintSetPtr_t& constraints);
151 
152  virtual bool impl_compute (ConfigurationOut_t result,
153  value_type param) const;
155  virtual void impl_derivative
156  (vectorOut_t result, const value_type& param, size_type order) const;
158  virtual void impl_velocityBound (vectorOut_t bound,
159  const value_type& param0,
160  const value_type& param1) const;
161 
163  virtual PathPtr_t impl_extract (const interval_t& paramInterval) const;
164 
166  void init (const ConstantCurvatureWkPtr_t& weak)
167  {
168  parent_t::init (weak);
169  weak_ = weak;
170  }
171 
173  ConstantCurvature() : curvature_(0), xyId_(0), rzId_(0) {}
174  private:
179  void setWheelJoints (const JointPtr_t rz,
180  const std::vector<JointPtr_t> wheels);
181 
182  DevicePtr_t robot_;
183  Configuration_t initial_;
184  Configuration_t end_;
185  value_type curveLength_;
186  const value_type curvature_;
187  const size_type xyId_,rzId_;
188  size_type dxyId_,drzId_;
189  value_type forward_;
190  std::vector<Wheels_t> wheels_;
191  ConstantCurvatureWkPtr_t weak_;
192 
193  HPP_SERIALIZABLE();
194  }; // class ConstantCurvature
195  } // namespace steeringMethod
196  } // namespace core
197 } // namespace hpp
198 
199 #endif // HPP_CORE_STEERING_METHOD_CONSTANT_CURVATURE_HH
virtual void impl_velocityBound(vectorOut_t bound, const value_type &param0, const value_type &param1) const
Virtual implementation of velocity bound.
Wheels_t()
Definition: constant-curvature.hh:94
virtual std::ostream & print(std::ostream &os) const
Print path in a stream.
virtual bool impl_compute(ConfigurationOut_t result, value_type param) const
Function evaluation without applying constraints.
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:114
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:97
Definition: bi-rrt-planner.hh:24
ConstantCurvature()
For serialization only.
Definition: constant-curvature.hh:173
std::pair< value_type, value_type > interval_t
Definition: fwd.hh:158
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:98
Path parent_t
Definition: constant-curvature.hh:32
pinocchio::size_type size_type
Definition: fwd.hh:156
static ConstantCurvaturePtr_t createCopy(const ConstantCurvaturePtr_t &other)
Create instance and return shared pointer.
pinocchio::JointPtr_t JointPtr_t
Definition: fwd.hh:133
const ConstraintSetPtr_t & constraints() const
Get constraints the path is subject to.
Definition: path.hh:226
virtual PathPtr_t copy(const ConstraintSetPtr_t &constraints) const
Definition: constant-curvature.hh:72
value_type value
Definition: constant-curvature.hh:92
shared_ptr< ConstantCurvature > ConstantCurvaturePtr_t
Definition: fwd.hh:32
Configuration_t end() const
Get the final configuration.
Definition: constant-curvature.hh:84
void init(const PathWkPtr_t &self)
Definition: constant-curvature.hh:91
JointPtr_t j
Definition: constant-curvature.hh:93
Path of constant curvature for a carlike robot.
Definition: constant-curvature.hh:30
void init(const ConstantCurvatureWkPtr_t &weak)
store weak pointer to itself
Definition: constant-curvature.hh:166
virtual PathPtr_t copy() const
Return a shared pointer to a copy of this.
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:110
pinocchio::value_type value_type
Definition: fwd.hh:157
Configuration_t initial() const
Get the initial configuration.
Definition: constant-curvature.hh:78
static ConstantCurvaturePtr_t create(const DevicePtr_t &robot, ConfigurationIn_t init, ConfigurationIn_t end, value_type curveLength, value_type pathLength, value_type curvature, size_type xyId, size_type rzId, const JointPtr_t rz, const std::vector< JointPtr_t > wheels, const ConstraintSetPtr_t &constraints)
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:204
virtual ~ConstantCurvature()
Definition: constant-curvature.hh:33
Definition: path.hh:60
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:96
virtual void impl_derivative(vectorOut_t result, const value_type &param, size_type order) const
Virtual implementation of derivative.
virtual PathPtr_t impl_extract(const interval_t &paramInterval) const
Virtual implementation of path extraction.
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:170