hpp-core  4.10.1
Implement basic classes for canonical path planning for kinematic chains.
straight-path.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2014 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_STRAIGHT_PATH_HH
20 # define HPP_CORE_STRAIGHT_PATH_HH
21 
22 # include <hpp/pinocchio/liegroup-element.hh>
23 
24 # include <hpp/core/fwd.hh>
25 # include <hpp/core/config.hh>
26 # include <hpp/core/path.hh>
27 
28 namespace hpp {
29  namespace core {
32 
43  {
44  public:
45  typedef Path parent_t;
47  virtual ~StraightPath () {}
48 
54  vectorIn_t init,
55  vectorIn_t end,
56  interval_t interval,
57  ConstraintSetPtr_t constraints = ConstraintSetPtr_t())
58  {
59  StraightPath* ptr;
60  if (constraints)
61  ptr = new StraightPath (space, init, end, interval, constraints);
62  else
63  ptr = new StraightPath (space, init, end, interval);
64  StraightPathPtr_t shPtr (ptr);
65  ptr->init (shPtr);
66  ptr->checkPath ();
67  return shPtr;
68  }
69 
76  interval_t interval,
77  ConstraintSetPtr_t constraints = ConstraintSetPtr_t())
78  {
79  assert (init.space() == end.space());
80  return create (init.space(), init.vector(), end.vector(), interval, constraints);
81  }
82 
87  static StraightPathPtr_t create (const DevicePtr_t& device,
88  ConfigurationIn_t init,
90  value_type length,
91  ConstraintSetPtr_t constraints = ConstraintSetPtr_t())
92  {
93  return create (device, init, end, interval_t (0, length), constraints);
94  }
95 
101  static StraightPathPtr_t create (const DevicePtr_t& device,
102  ConfigurationIn_t init,
103  ConfigurationIn_t end,
104  interval_t interval,
105  ConstraintSetPtr_t constraints = ConstraintSetPtr_t());
106 
110  {
111  StraightPath* ptr = new StraightPath (*path);
112  StraightPathPtr_t shPtr (ptr);
113  ptr->init (shPtr);
114  ptr->checkPath ();
115  return shPtr;
116  }
117 
121  static StraightPathPtr_t createCopy
122  (const StraightPathPtr_t& path, const ConstraintSetPtr_t& constraints)
123  {
124  StraightPath* ptr = new StraightPath (*path, constraints);
125  StraightPathPtr_t shPtr (ptr);
126  ptr->init (shPtr);
127  ptr->checkPath ();
128  return shPtr;
129  }
130 
135  virtual PathPtr_t copy () const
136  {
137  return createCopy (weak_.lock ());
138  }
139 
144  virtual PathPtr_t copy (const ConstraintSetPtr_t& constraints) const
145  {
146  return createCopy (weak_.lock (), constraints);
147  }
148 
154  {
155  assert (initial.size () == initial_.size ());
156  initial_ = initial;
157  }
158 
164  {
165  assert (end.size () == end_.size ());
166  end_ = end;
167  }
168 
171  {
172  return initial_;
173  }
174 
177  {
178  return end_;
179  }
180 
181  protected:
183  virtual std::ostream& print (std::ostream &os) const;
184 
187  interval_t interval);
188 
191  interval_t interval, ConstraintSetPtr_t constraints);
192 
194  StraightPath (const StraightPath& path);
195 
197  StraightPath (const StraightPath& path,
198  const ConstraintSetPtr_t& constraints);
199 
201  {
202  parent_t::init (self);
203  weak_ = self;
204  checkPath ();
205  }
206 
207  virtual bool impl_compute (ConfigurationOut_t result,
208  value_type param) const;
210  virtual void impl_derivative (vectorOut_t result, const value_type& t,
211  size_type order) const;
212 
213  virtual void impl_velocityBound (vectorOut_t result, const value_type&, const value_type&) const;
214 
219  PathPtr_t impl_extract (const interval_t& subInterval) const;
220 
221  protected:
225  private:
226  StraightPathWkPtr_t weak_;
227 
228  protected:
230  private:
231  HPP_SERIALIZABLE();
232  }; // class StraightPath
234  } // namespace core
235 } // namespace hpp
236 #endif // HPP_CORE_STRAIGHT_PATH_HH
boost::shared_ptr< Path > PathPtr_t
Definition: fwd.hh:170
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:114
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:97
void init(StraightPathPtr_t self)
Definition: straight-path.hh:200
Configuration_t initial_
Definition: straight-path.hh:223
Definition: basic-configuration-shooter.hh:26
boost::shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:110
std::pair< value_type, value_type > interval_t
Definition: fwd.hh:158
virtual void checkPath() const
Should be called by child classes after having init.
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:98
pinocchio::size_type size_type
Definition: fwd.hh:156
void endConfig(ConfigurationIn_t end)
Definition: straight-path.hh:163
virtual PathPtr_t copy() const
Definition: straight-path.hh:135
static StraightPathPtr_t createCopy(const StraightPathPtr_t &path)
Definition: straight-path.hh:109
static StraightPathPtr_t create(LiegroupSpacePtr_t space, vectorIn_t init, vectorIn_t end, interval_t interval, ConstraintSetPtr_t constraints=ConstraintSetPtr_t())
Definition: straight-path.hh:53
Path parent_t
Definition: straight-path.hh:45
void initialConfig(ConfigurationIn_t initial)
Definition: straight-path.hh:153
Configuration_t end() const
Get the final configuration.
Definition: straight-path.hh:176
boost::shared_ptr< StraightPath > StraightPathPtr_t
Definition: fwd.hh:182
Configuration_t initial() const
Get the initial configuration.
Definition: straight-path.hh:170
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:202
constraints::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:155
pinocchio::LiegroupElementConstRef LiegroupElementConstRef
Definition: fwd.hh:153
virtual ~StraightPath()
Destructor.
Definition: straight-path.hh:47
pinocchio::value_type value_type
Definition: fwd.hh:157
Configuration_t end_
Definition: straight-path.hh:224
Definition: straight-path.hh:42
static StraightPathPtr_t create(LiegroupElementConstRef init, LiegroupElementConstRef end, interval_t interval, ConstraintSetPtr_t constraints=ConstraintSetPtr_t())
Definition: straight-path.hh:74
LiegroupSpacePtr_t space_
Definition: straight-path.hh:222
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:203
virtual PathPtr_t copy(const ConstraintSetPtr_t &constraints) const
Definition: straight-path.hh:144
static StraightPathPtr_t create(const DevicePtr_t &device, ConfigurationIn_t init, ConfigurationIn_t end, value_type length, ConstraintSetPtr_t constraints=ConstraintSetPtr_t())
Definition: straight-path.hh:87
Definition: path.hh:61
#define HPP_CORE_DLLAPI
Definition: config.hh:64
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:96
StraightPath()
Definition: straight-path.hh:229