hpp-core: include/hpp/core/kinodynamic-oriented-path.hh Source File

hpp-core 9.0.2 Implement basic classes for canonical path planning for kinematic chains.

Loading...
Searching...
No Matches
kinodynamic-oriented-path.hh
Go to the documentation of this file.
1// Copyright (c) 2016, LAAS-CNRS
2// Authors: Pierre Fernbach (pierre.fernbach@laas.fr)
3//
4
5// Redistribution and use in source and binary forms, with or without
6// modification, are permitted provided that the following conditions are
7// met:
8//
9// 1. Redistributions of source code must retain the above copyright
10// notice, this list of conditions and the following disclaimer.
11//
12// 2. Redistributions in binary form must reproduce the above copyright
13// notice, this list of conditions and the following disclaimer in the
14// documentation and/or other materials provided with the distribution.
15//
16// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27// DAMAGE.
28
29#ifndef HPP_CORE_KINODYNAMIC_ORIENTED_PATH_HH
30#define HPP_CORE_KINODYNAMIC_ORIENTED_PATH_HH
31
32#include <hpp/core/kinodynamic-path.hh>
33
34namespace hpp {
35namespace core {
41
42 class HPP_CORE_DLLAPI KinodynamicOrientedPath : public KinodynamicPath {
43 public:
44 typedef KinodynamicPath parent_t;
45 virtual ~KinodynamicOrientedPath() {}
46
51 static KinodynamicOrientedPathPtr_t create(
52 const DevicePtr_t& device, ConfigurationIn_t init, ConfigurationIn_t end,
53 value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0,
54 ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2,
55 ConfigurationIn_t vLim, bool ignoreZValue = false) {
56 KinodynamicOrientedPath* ptr = new KinodynamicOrientedPath(
57 device, init, end, length, a1, t0, t1, tv, t2, vLim, ignoreZValue);
58 KinodynamicOrientedPathPtr_t shPtr(ptr);
59 ptr->init(shPtr);
60 ptr->checkPath();
61 return shPtr;
62 }
63
69 static KinodynamicOrientedPathPtr_t create(
70 const DevicePtr_t& device, ConfigurationIn_t init, ConfigurationIn_t end,
71 value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0,
72 ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2,
73 ConfigurationIn_t vLim, ConstraintSetPtr_t constraints,
74 bool ignoreZValue = false) {
75 KinodynamicOrientedPath* ptr =
76 new KinodynamicOrientedPath(device, init, end, length, a1, t0, t1, tv,
77 t2, vLim, constraints, ignoreZValue);
78 KinodynamicOrientedPathPtr_t shPtr(ptr);
79 ptr->init(shPtr);
80 ptr->checkPath();
81 return shPtr;
82 }
83
86 static KinodynamicOrientedPathPtr_t createCopy(
87 const KinodynamicOrientedPathPtr_t& path) {
88 KinodynamicOrientedPath* ptr = new KinodynamicOrientedPath(*path);
89 KinodynamicOrientedPathPtr_t shPtr(ptr);
90 ptr->init(shPtr);
91 ptr->checkPath();
92 return shPtr;
93 }
94
95 static KinodynamicOrientedPathPtr_t create(const KinodynamicPathPtr_t& path,
96 bool ignoreZValue = false) {
97 KinodynamicOrientedPath* ptr =
98 new KinodynamicOrientedPath(*path, ignoreZValue);
99 KinodynamicOrientedPathPtr_t shPtr(ptr);
100 ptr->init(shPtr);
101 ptr->checkPath();
102 return shPtr;
103 }
104
108 static KinodynamicOrientedPathPtr_t createCopy(
109 const KinodynamicOrientedPathPtr_t& path,
110 const ConstraintSetPtr_t& constraints) {
111 KinodynamicOrientedPath* ptr =
112 new KinodynamicOrientedPath(*path, constraints);
113 KinodynamicOrientedPathPtr_t shPtr(ptr);
114 ptr->init(shPtr);
115 ptr->checkPath();
116 return shPtr;
117 }
118
123 virtual PathPtr_t copy() const { return createCopy(weak_.lock()); }
124
129 virtual PathPtr_t copy(const ConstraintSetPtr_t& constraints) const {
130 return createCopy(weak_.lock(), constraints);
131 }
132
133 bool ignoreZValue() const { return ignoreZValue_; }
134 void ignoreZValue(bool ignoreZValue) { ignoreZValue_ = ignoreZValue; }
135
136 protected:
137 // Make this method accessible to this class
138 void constraints(const ConstraintSetPtr_t& constr) {
139 parent_t::constraints(constr);
140 }
142 virtual std::ostream& print(std::ostream& os) const {
143 os << "KinodynamicOrientedPath:" << std::endl;
144 os << "interval: [ " << timeRange().first << ", " << timeRange().second
145 << " ]" << std::endl;
146 os << "initial configuration: " << pinocchio::displayConfig(initial_)
147 << std::endl;
148 os << "final configuration: " << pinocchio::displayConfig(end_)
149 << std::endl;
150 return os;
151 }
152
153 void orienteInitAndGoal(const DevicePtr_t& device);
154
156 KinodynamicOrientedPath(const DevicePtr_t& robot, ConfigurationIn_t init,
157 ConfigurationIn_t end, value_type length,
158 ConfigurationIn_t a1, ConfigurationIn_t t0,
159 ConfigurationIn_t t1, ConfigurationIn_t tv,
160 ConfigurationIn_t t2, ConfigurationIn_t vLim,
161 bool ignoreZValue);
162
164 KinodynamicOrientedPath(const DevicePtr_t& robot, ConfigurationIn_t init,
165 ConfigurationIn_t end, value_type length,
166 ConfigurationIn_t a1, ConfigurationIn_t t0,
167 ConfigurationIn_t t1, ConfigurationIn_t tv,
168 ConfigurationIn_t t2, ConfigurationIn_t vLim,
169 ConstraintSetPtr_t constraints, bool ignoreZValue);
170
172 KinodynamicOrientedPath(const KinodynamicOrientedPath& path);
173
175 KinodynamicOrientedPath(const KinodynamicPath& path, bool ignoreZValue);
176
178 KinodynamicOrientedPath(const KinodynamicOrientedPath& path,
179 const ConstraintSetPtr_t& constraints);
180
181 void init(KinodynamicOrientedPathPtr_t self) {
182 parent_t::init(self);
183 weak_ = self;
184 checkPath();
185 }
186
187 virtual bool impl_compute(ConfigurationOut_t result, value_type t) const;
188
189 virtual PathPtr_t impl_extract(const interval_t& subInterval) const;
190
191 private:
192 KinodynamicOrientedPathWkPtr_t weak_;
193 bool ignoreZValue_;
194}; // class kinodynamic oriented path
195} // namespace core
196} // namespace hpp
197
198#endif // HPP_CORE_KINODYNAMIC_ORIENTED_PATH_HH
hpp::core::KinodynamicOrientedPath
Definition kinodynamic-oriented-path.hh:42
hpp::core::KinodynamicOrientedPath::KinodynamicOrientedPath
KinodynamicOrientedPath(const DevicePtr_t &robot, ConfigurationIn_t init, ConfigurationIn_t end, value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0, ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2, ConfigurationIn_t vLim, bool ignoreZValue)
Constructor.
hpp::core::KinodynamicOrientedPath::createCopy
static KinodynamicOrientedPathPtr_t createCopy(const KinodynamicOrientedPathPtr_t &path, const ConstraintSetPtr_t &constraints)
Definition kinodynamic-oriented-path.hh:108
hpp::core::KinodynamicOrientedPath::ignoreZValue
void ignoreZValue(bool ignoreZValue)
Definition kinodynamic-oriented-path.hh:134
hpp::core::KinodynamicOrientedPath::KinodynamicOrientedPath
KinodynamicOrientedPath(const DevicePtr_t &robot, ConfigurationIn_t init, ConfigurationIn_t end, value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0, ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2, ConfigurationIn_t vLim, ConstraintSetPtr_t constraints, bool ignoreZValue)
Constructor with constraints.
hpp::core::KinodynamicOrientedPath::constraints
void constraints(const ConstraintSetPtr_t &constr)
Definition kinodynamic-oriented-path.hh:138
hpp::core::KinodynamicOrientedPath::parent_t
KinodynamicPath parent_t
Definition kinodynamic-oriented-path.hh:44
hpp::core::KinodynamicOrientedPath::ignoreZValue
bool ignoreZValue() const
Definition kinodynamic-oriented-path.hh:133
hpp::core::KinodynamicOrientedPath::print
virtual std::ostream & print(std::ostream &os) const
Print path in a stream.
Definition kinodynamic-oriented-path.hh:142
hpp::core::KinodynamicOrientedPath::create
static KinodynamicOrientedPathPtr_t create(const DevicePtr_t &device, ConfigurationIn_t init, ConfigurationIn_t end, value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0, ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2, ConfigurationIn_t vLim, bool ignoreZValue=false)
Definition kinodynamic-oriented-path.hh:51
hpp::core::KinodynamicOrientedPath::init
void init(KinodynamicOrientedPathPtr_t self)
Definition kinodynamic-oriented-path.hh:181
hpp::core::KinodynamicOrientedPath::copy
virtual PathPtr_t copy() const
Definition kinodynamic-oriented-path.hh:123
hpp::core::KinodynamicOrientedPath::KinodynamicOrientedPath
KinodynamicOrientedPath(const KinodynamicOrientedPath &path, const ConstraintSetPtr_t &constraints)
Copy constructor with constraints.
hpp::core::KinodynamicOrientedPath::orienteInitAndGoal
void orienteInitAndGoal(const DevicePtr_t &device)
hpp::core::KinodynamicOrientedPath::~KinodynamicOrientedPath
virtual ~KinodynamicOrientedPath()
Definition kinodynamic-oriented-path.hh:45
hpp::core::KinodynamicOrientedPath::createCopy
static KinodynamicOrientedPathPtr_t createCopy(const KinodynamicOrientedPathPtr_t &path)
Definition kinodynamic-oriented-path.hh:86
hpp::core::KinodynamicOrientedPath::impl_extract
virtual PathPtr_t impl_extract(const interval_t &subInterval) const
hpp::core::KinodynamicOrientedPath::copy
virtual PathPtr_t copy(const ConstraintSetPtr_t &constraints) const
Definition kinodynamic-oriented-path.hh:129
hpp::core::KinodynamicOrientedPath::impl_compute
virtual bool impl_compute(ConfigurationOut_t result, value_type t) const
Function evaluation without applying constraints.
hpp::core::KinodynamicOrientedPath::create
static KinodynamicOrientedPathPtr_t create(const KinodynamicPathPtr_t &path, bool ignoreZValue=false)
Definition kinodynamic-oriented-path.hh:95
hpp::core::KinodynamicOrientedPath::create
static KinodynamicOrientedPathPtr_t create(const DevicePtr_t &device, ConfigurationIn_t init, ConfigurationIn_t end, value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0, ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2, ConfigurationIn_t vLim, ConstraintSetPtr_t constraints, bool ignoreZValue=false)
Definition kinodynamic-oriented-path.hh:69
hpp::core::KinodynamicOrientedPath::KinodynamicOrientedPath
KinodynamicOrientedPath(const KinodynamicPath &path, bool ignoreZValue)
constructor from KinodynamicPath
hpp::core::KinodynamicOrientedPath::KinodynamicOrientedPath
KinodynamicOrientedPath(const KinodynamicOrientedPath &path)
Copy constructor.
hpp::core::KinodynamicPath
Definition kinodynamic-path.hh:59
hpp::core::Path::checkPath
virtual void checkPath() const
Should be called by child classes after having init.
HPP_CORE_DLLAPI
#define HPP_CORE_DLLAPI
Definition config.hh:88
kinodynamic-path.hh
hpp
hpp::core::value_type
pinocchio::value_type value_type
Definition fwd.hh:174
hpp::core::ConfigurationOut_t
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition fwd.hh:109
hpp::core::ConfigurationIn_t
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:108
hpp::core::KinodynamicPathPtr_t
shared_ptr< KinodynamicPath > KinodynamicPathPtr_t
Definition fwd.hh:206
hpp::core::interval_t
std::pair< value_type, value_type > interval_t
Definition fwd.hh:175
hpp::core::KinodynamicOrientedPathPtr_t
shared_ptr< KinodynamicOrientedPath > KinodynamicOrientedPathPtr_t
Definition fwd.hh:208
hpp::core::DevicePtr_t
pinocchio::DevicePtr_t DevicePtr_t
Definition fwd.hh:134
hpp::core::ConstraintSetPtr_t
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition fwd.hh:130
hpp::core::PathPtr_t
shared_ptr< Path > PathPtr_t
Definition fwd.hh:187