hpp-rbprm 4.15.1
Implementation of RB-PRM planner using hpp.
Loading...
Searching...
No Matches
time-constraint-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_RBPRM_TIMECONSTRAINT_PATH_HH
20#define HPP_RBPRM_TIMECONSTRAINT_PATH_HH
21
22#include <hpp/core/config.hh>
23#include <hpp/core/fwd.hh>
24#include <hpp/core/path.hh>
26
27namespace hpp {
28namespace rbprm {
29namespace interpolation {
31typedef shared_ptr<TimeConstraintPath> TimeConstraintPathPtr_t;
41class HPP_CORE_DLLAPI TimeConstraintPath : public core::Path {
42 public:
43 typedef Path parent_t;
46
51 static TimeConstraintPathPtr_t create(const core::DevicePtr_t& device,
52 core::ConfigurationIn_t init,
53 core::ConfigurationIn_t end,
54 core::value_type length,
55 const std::size_t pathDofRank,
56 const T_TimeDependant& tds) {
58 new TimeConstraintPath(device, init, end, length, pathDofRank, tds);
59 TimeConstraintPathPtr_t shPtr(ptr);
60 ptr->init(shPtr);
61 ptr->checkPath();
62 return shPtr;
63 }
64
70 static TimeConstraintPathPtr_t create(const core::DevicePtr_t& device,
71 core::ConfigurationIn_t init,
72 core::ConfigurationIn_t end,
73 core::value_type length,
74 core::ConstraintSetPtr_t constraints,
75 const std::size_t pathDofRank,
76 const T_TimeDependant& tds) {
78 device, init, end, length, constraints, pathDofRank, tds);
79 TimeConstraintPathPtr_t shPtr(ptr);
80 ptr->init(shPtr);
81 ptr->checkPath();
82 return shPtr;
83 }
84
88 const TimeConstraintPathPtr_t& path) {
89 TimeConstraintPath* ptr = new TimeConstraintPath(*path);
90 TimeConstraintPathPtr_t shPtr(ptr);
91 ptr->initCopy(shPtr);
92 ptr->checkPath();
93 return shPtr;
94 }
95
100 const TimeConstraintPathPtr_t& path,
101 const core::ConstraintSetPtr_t& constraints) {
102 TimeConstraintPath* ptr = new TimeConstraintPath(*path, constraints);
103 TimeConstraintPathPtr_t shPtr(ptr);
104 ptr->initCopy(shPtr);
105 ptr->checkPath();
106 return shPtr;
107 }
108
113 virtual core::PathPtr_t copy() const { return createCopy(weak_.lock()); }
114
119 virtual core::PathPtr_t copy(
120 const core::ConstraintSetPtr_t& constraints) const {
121 return createCopy(weak_.lock(), constraints);
122 }
123
128 virtual core::PathPtr_t extract(const core::interval_t& subInterval) const;
129
134 void initialConfig(core::ConfigurationIn_t initial) {
135 assert(initial.size() == initial_.size());
136 pinocchio::value_type dof = initial_[pathDofRank_];
137 initial_ = initial;
138 initial_[pathDofRank_] = dof;
139 }
140
145 void endConfig(core::ConfigurationIn_t end) {
146 assert(end.size() == end_.size());
147 pinocchio::value_type dof = end_[pathDofRank_];
148 end_ = end;
149 end_[pathDofRank_] = dof;
150 }
151
153 core::DevicePtr_t device() const;
154
156 core::Configuration_t initial() const { return initial_; }
157
159 core::Configuration_t end() const { return end_; }
160
161 virtual void checkPath() const;
162
163 protected:
165 virtual std::ostream& print(std::ostream& os) const {
166 os << "TimeConstraintPath:" << std::endl;
167 os << "interval: [ " << timeRange().first << ", " << timeRange().second
168 << " ]" << std::endl;
169 os << "initial configuration: " << initial_.transpose() << std::endl;
170 os << "final configuration: " << end_.transpose() << std::endl;
171 return os;
172 }
174 TimeConstraintPath(const core::DevicePtr_t& robot,
175 core::ConfigurationIn_t init, core::ConfigurationIn_t end,
176 core::value_type length, const std::size_t pathDofRank,
177 const T_TimeDependant& tds);
178
180 TimeConstraintPath(const core::DevicePtr_t& robot,
181 core::ConfigurationIn_t init, core::ConfigurationIn_t end,
182 core::value_type length,
183 core::ConstraintSetPtr_t constraints,
184 const std::size_t pathDofRank, const T_TimeDependant& tds);
185
188
191 const core::ConstraintSetPtr_t& constraints);
192
194 parent_t::init(self);
195 weak_ = self;
196 }
197
199 parent_t::init(self);
200 weak_ = self;
201 }
202
203 virtual bool impl_compute(core::ConfigurationOut_t result,
204 core::value_type param) const;
205
206 private:
207 void updateConstraints(core::ConfigurationOut_t configuration) const;
208
209 private:
210 core::DevicePtr_t device_;
211 core::Configuration_t initial_;
212 core::Configuration_t end_;
213
214 public:
215 const std::size_t pathDofRank_;
217
218 private:
219 TimeConstraintPathWkPtr_t weak_;
220}; // class TimeConstraintPath
221} // namespace interpolation
222} // namespace rbprm
223} // namespace hpp
224#endif // HPP_RBPRM_TIMECONSTRAINT_PATH_HH
Definition: time-constraint-path.hh:41
static TimeConstraintPathPtr_t create(const core::DevicePtr_t &device, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::value_type length, const std::size_t pathDofRank, const T_TimeDependant &tds)
Definition: time-constraint-path.hh:51
core::DevicePtr_t device() const
Return the internal robot.
const std::size_t pathDofRank_
Definition: time-constraint-path.hh:215
TimeConstraintPath(const TimeConstraintPath &path, const core::ConstraintSetPtr_t &constraints)
Copy constructor with constraints.
virtual std::ostream & print(std::ostream &os) const
Print path in a stream.
Definition: time-constraint-path.hh:165
virtual ~TimeConstraintPath()
Destructor.
Definition: time-constraint-path.hh:45
const T_TimeDependant tds_
Definition: time-constraint-path.hh:216
static TimeConstraintPathPtr_t createCopy(const TimeConstraintPathPtr_t &path)
Definition: time-constraint-path.hh:87
static TimeConstraintPathPtr_t create(const core::DevicePtr_t &device, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::value_type length, core::ConstraintSetPtr_t constraints, const std::size_t pathDofRank, const T_TimeDependant &tds)
Definition: time-constraint-path.hh:70
void init(TimeConstraintPathPtr_t self)
Definition: time-constraint-path.hh:193
virtual core::PathPtr_t copy(const core::ConstraintSetPtr_t &constraints) const
Definition: time-constraint-path.hh:119
TimeConstraintPath(const TimeConstraintPath &path)
Copy constructor.
core::Configuration_t initial() const
Get the initial configuration.
Definition: time-constraint-path.hh:156
virtual core::PathPtr_t copy() const
Definition: time-constraint-path.hh:113
virtual core::PathPtr_t extract(const core::interval_t &subInterval) const
void initialConfig(core::ConfigurationIn_t initial)
Definition: time-constraint-path.hh:134
void endConfig(core::ConfigurationIn_t end)
Definition: time-constraint-path.hh:145
Path parent_t
Definition: time-constraint-path.hh:43
virtual bool impl_compute(core::ConfigurationOut_t result, core::value_type param) const
TimeConstraintPath(const core::DevicePtr_t &robot, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::value_type length, const std::size_t pathDofRank, const T_TimeDependant &tds)
Constructor.
TimeConstraintPath(const core::DevicePtr_t &robot, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::value_type length, core::ConstraintSetPtr_t constraints, const std::size_t pathDofRank, const T_TimeDependant &tds)
Constructor with constraints.
core::Configuration_t end() const
Get the final configuration.
Definition: time-constraint-path.hh:159
static TimeConstraintPathPtr_t createCopy(const TimeConstraintPathPtr_t &path, const core::ConstraintSetPtr_t &constraints)
Definition: time-constraint-path.hh:99
void initCopy(TimeConstraintPathPtr_t self)
Definition: time-constraint-path.hh:198
HPP_PREDEF_CLASS(ComTrajectory)
shared_ptr< TimeConstraintPath > TimeConstraintPathPtr_t
Definition: time-constraint-path.hh:31
std::vector< TimeDependant > T_TimeDependant
Definition: time-dependant.hh:73
Definition: algorithm.hh:26