hpp-rbprm  4.10.0
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/path-validation/discretized.hh>
23 # include <hpp/core/steering-method/straight.hh>
24 # include <hpp/core/straight-path.hh>
25 # include <hpp/core/problem.hh>
26 # include <hpp/core/distance.hh>
27 # include <hpp/core/config-projector.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  {
43  typedef Path_T path_t;
44  typedef boost::shared_ptr <TimeConstraintSteering> TimeConstraintSteeringPtr_t;
45  typedef boost::weak_ptr <TimeConstraintSteering> TimeConstraintSteeringWkPtr_t;
46  public:
48  static TimeConstraintSteeringPtr_t create (const core::ProblemPtr_t& problem,
49  const std::size_t pathDofRank)
50  {
51  TimeConstraintSteering* ptr = 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, const core::WeighedDistancePtr_t& distance,
59  const std::size_t pathDofRank)
60  HPP_CORE_DEPRECATED
61  {
63  distance, pathDofRank);
64  TimeConstraintSteeringPtr_t shPtr (ptr);
65  ptr->init (shPtr);
66  return shPtr;
67  }
69  static TimeConstraintSteeringPtr_t createCopy
70  (const TimeConstraintSteeringPtr_t& other)
71  {
73  TimeConstraintSteeringPtr_t shPtr (ptr);
74  ptr->init (shPtr);
75  return shPtr;
76  }
78  virtual core::SteeringMethodPtr_t copy () const
79  {
80  return createCopy (weak_.lock ());
81  }
82 
84  virtual core::PathPtr_t impl_compute (core::ConfigurationIn_t q1,
85  core::ConfigurationIn_t q2) const
86  {
87  core::value_type length = problem_.distance()->operator () (q1, q2);
88  core::ConstraintSetPtr_t c;
89  if (constraints() && constraints()->configProjector ()) {
90  c = HPP_STATIC_PTR_CAST (core::ConstraintSet, constraints()->copy ());
91  c->configProjector()->rightHandSideFromConfig (q1);
92  } else {
93  c = constraints ();
94  }
95  core::PathPtr_t path = path_t::create
96  (problem_.robot(), q1, q2, length, c, pathDofRank_, tds_);
97  return path;
98  }
99  protected:
102  TimeConstraintSteering (const core::ProblemPtr_t& problem,
103  const std::size_t pathDofRank) :
104  core::steeringMethod::Straight(*problem), pathDofRank_(pathDofRank), weak_ () {}
105 
106  /*/// Constructor with weighed distance
107  TimeConstraintSteering (const core::DevicePtr_t& device,
108  const core::WeighedDistancePtr_t& distance,
109  const std::size_t pathDofRank) :
110  SteeringMethod (new core::Problem (device)), pathDofRank_(pathDofRank), weak_ ()
111  {
112  problem_.distance (distance);
113  }*/
116  core::steeringMethod::Straight (other), pathDofRank_(other.pathDofRank_), weak_ (), tds_(other.tds_) {}
117 
119  void init (TimeConstraintSteeringWkPtr_t weak)
120  {
121  core::steeringMethod::Straight::init (weak);
122  weak_ = weak;
123  }
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
hpp::rbprm::interpolation::TimeConstraintSteering::TimeConstraintSteering
TimeConstraintSteering(const core::ProblemPtr_t &problem, const std::size_t pathDofRank)
Definition: time-constraint-steering.hh:102
hpp::rbprm::interpolation::TimeConstraintSteering::copy
virtual core::SteeringMethodPtr_t copy() const
Copy instance and return shared pointer.
Definition: time-constraint-steering.hh:78
hpp::rbprm::interpolation::T_TimeDependant
std::vector< TimeDependant > T_TimeDependant
Definition: time-dependant.hh:75
hpp::tools::distance
pinocchio::value_type distance(pinocchio::ConfigurationIn_t q1, pinocchio::ConfigurationIn_t q2)
time-constraint-path.hh
hpp::rbprm::interpolation::TimeConstraintSteering::impl_compute
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:84
hpp::rbprm::interpolation::TimeConstraintSteering::createCopy
static TimeConstraintSteeringPtr_t createCopy(const TimeConstraintSteeringPtr_t &other)
Copy instance and return shared pointer.
Definition: time-constraint-steering.hh:70
hpp::rbprm::interpolation::TimeConstraintSteering::init
void init(TimeConstraintSteeringWkPtr_t weak)
Store weak pointer to itself.
Definition: time-constraint-steering.hh:119
time-dependant.hh
hpp::rbprm::interpolation::TimeConstraintSteering::tds_
T_TimeDependant tds_
Definition: time-constraint-steering.hh:132
hpp::rbprm::interpolation::TimeConstraintSteering::TimeConstraintSteering
TimeConstraintSteering(const TimeConstraintSteering &other)
Copy constructor.
Definition: time-constraint-steering.hh:115
hpp
Definition: algorithm.hh:27
hpp::rbprm::interpolation::TimeConstraintSteering
Definition: time-constraint-steering.hh:41
hpp::rbprm::interpolation::TimeConstraintSteering::create
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