hpp-rbprm  4.15.1
Implementation of RB-PRM planner using hpp.
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 
31 namespace hpp {
32 namespace rbprm {
33 namespace interpolation {
40 template <class Path_T>
41 class 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) :
109 SteeringMethod (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
pinocchio::value_type distance(pinocchio::ConfigurationIn_t q1, pinocchio::ConfigurationIn_t q2)
Definition: algorithm.hh:26