hpp-rbprm 4.15.1
Implementation of RB-PRM planner using hpp.
Loading...
Searching...
No Matches
time-constraint-steering.hh
Go to the documentation of this file.
1//
2// Copyright (c) 2015 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_TIME_CONSTRAINT_STEERING_HH
20#define HPP_TIME_CONSTRAINT_STEERING_HH
21
22#include <hpp/core/config-projector.hh>
23#include <hpp/core/distance.hh>
24#include <hpp/core/path-validation/discretized.hh>
25#include <hpp/core/problem.hh>
26#include <hpp/core/steering-method/straight.hh>
27#include <hpp/core/straight-path.hh>
30
31namespace hpp {
32namespace rbprm {
33namespace interpolation {
40template <class Path_T>
41class TimeConstraintSteering : public hpp::core::steeringMethod::Straight {
42 typedef Path_T path_t;
43 typedef shared_ptr<TimeConstraintSteering> TimeConstraintSteeringPtr_t;
44 typedef weak_ptr<TimeConstraintSteering> TimeConstraintSteeringWkPtr_t;
45
46 public:
48 static TimeConstraintSteeringPtr_t create(const core::ProblemPtr_t& problem,
49 const std::size_t pathDofRank) {
51 new TimeConstraintSteering(problem, pathDofRank);
52 TimeConstraintSteeringPtr_t shPtr(ptr);
53 ptr->init(shPtr);
54 return shPtr;
55 }
57 static TimeConstraintSteeringPtr_t create(
58 const core::DevicePtr_t& device,
59 const core::WeighedDistancePtr_t& distance,
60 const std::size_t pathDofRank) HPP_CORE_DEPRECATED {
62 new TimeConstraintSteering(device, distance, pathDofRank);
63 TimeConstraintSteeringPtr_t shPtr(ptr);
64 ptr->init(shPtr);
65 return shPtr;
66 }
68 static TimeConstraintSteeringPtr_t createCopy(
69 const TimeConstraintSteeringPtr_t& other) {
71 TimeConstraintSteeringPtr_t shPtr(ptr);
72 ptr->init(shPtr);
73 return shPtr;
74 }
76 virtual core::SteeringMethodPtr_t copy() const {
77 return createCopy(weak_.lock());
78 }
79
81 virtual core::PathPtr_t impl_compute(core::ConfigurationIn_t q1,
82 core::ConfigurationIn_t q2) const {
83 core::value_type length = problem()->distance()->operator()(q1, q2);
84 core::ConstraintSetPtr_t c;
85 if (constraints() && constraints()->configProjector()) {
86 c = HPP_STATIC_PTR_CAST(core::ConstraintSet, constraints()->copy());
87 c->configProjector()->rightHandSideFromConfig(q1);
88 } else {
89 c = constraints();
90 }
91 core::PathPtr_t path = path_t::create(problem()->robot(), q1, q2, length, c,
92 pathDofRank_, tds_);
93 return path;
94 }
95
96 protected:
99 TimeConstraintSteering(const core::ProblemPtr_t& problem,
100 const std::size_t pathDofRank)
101 : core::steeringMethod::Straight(problem),
102 pathDofRank_(pathDofRank),
103 weak_() {}
104
105 /*/// Constructor with weighed distance
106 TimeConstraintSteering (const core::DevicePtr_t& device,
107 const core::WeighedDistancePtr_t& distance,
108 const std::size_t pathDofRank) :
109SteeringMethod (new core::Problem (device)), pathDofRank_(pathDofRank), weak_ ()
110 {
111 problem().distance (distance);
112 }*/
115 : core::steeringMethod::Straight(other),
116 pathDofRank_(other.pathDofRank_),
117 weak_(),
118 tds_(other.tds_) {}
119
121 void init(TimeConstraintSteeringWkPtr_t weak) {
122 core::steeringMethod::Straight::init(weak);
123 weak_ = weak;
124 }
125
126 private:
127 const core::PathPtr_t model_;
128 const std::size_t pathDofRank_;
129 TimeConstraintSteeringWkPtr_t weak_;
130
131 public:
133}; // SteeringMethodStraight
135} // namespace interpolation
136} // namespace rbprm
137} // namespace hpp
138
139#endif // HPP_TIME_CONSTRAINT_STEERING_HH
Definition: time-constraint-steering.hh:41
virtual core::SteeringMethodPtr_t copy() const
Copy instance and return shared pointer.
Definition: time-constraint-steering.hh:76
TimeConstraintSteering(const core::ProblemPtr_t &problem, const std::size_t pathDofRank)
Definition: time-constraint-steering.hh:99
virtual core::PathPtr_t impl_compute(core::ConfigurationIn_t q1, core::ConfigurationIn_t q2) const
create a path between two configurations
Definition: time-constraint-steering.hh:81
void init(TimeConstraintSteeringWkPtr_t weak)
Store weak pointer to itself.
Definition: time-constraint-steering.hh:121
static TimeConstraintSteeringPtr_t createCopy(const TimeConstraintSteeringPtr_t &other)
Copy instance and return shared pointer.
Definition: time-constraint-steering.hh:68
static TimeConstraintSteeringPtr_t create(const core::DevicePtr_t &device, const core::WeighedDistancePtr_t &distance, const std::size_t pathDofRank) HPP_CORE_DEPRECATED
Create instance and return shared pointer.
Definition: time-constraint-steering.hh:57
static TimeConstraintSteeringPtr_t create(const core::ProblemPtr_t &problem, const std::size_t pathDofRank)
Create instance and return shared pointer.
Definition: time-constraint-steering.hh:48
TimeConstraintSteering(const TimeConstraintSteering &other)
Copy constructor.
Definition: time-constraint-steering.hh:114
T_TimeDependant tds_
Definition: time-constraint-steering.hh:132
std::vector< TimeDependant > T_TimeDependant
Definition: time-dependant.hh:73
Definition: algorithm.hh:26