19 #ifndef HPP_TIME_CONSTRAINT_STEERING_HH
20 #define HPP_TIME_CONSTRAINT_STEERING_HH
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>
33 namespace interpolation {
40 template <
class Path_T>
42 typedef Path_T path_t;
43 typedef boost::shared_ptr<TimeConstraintSteering> TimeConstraintSteeringPtr_t;
44 typedef boost::weak_ptr<TimeConstraintSteering> TimeConstraintSteeringWkPtr_t;
48 static TimeConstraintSteeringPtr_t
create(
const core::ProblemPtr_t& problem,
const std::size_t pathDofRank) {
50 TimeConstraintSteeringPtr_t shPtr(ptr);
55 static TimeConstraintSteeringPtr_t
create(
const core::DevicePtr_t& device,
56 const core::WeighedDistancePtr_t&
distance,
57 const std::size_t pathDofRank) HPP_CORE_DEPRECATED {
59 TimeConstraintSteeringPtr_t shPtr(ptr);
64 static TimeConstraintSteeringPtr_t
createCopy(
const TimeConstraintSteeringPtr_t& other) {
66 TimeConstraintSteeringPtr_t shPtr(ptr);
71 virtual core::SteeringMethodPtr_t
copy()
const {
return createCopy(weak_.lock()); }
74 virtual core::PathPtr_t
impl_compute(core::ConfigurationIn_t q1, core::ConfigurationIn_t q2)
const {
75 core::value_type length = problem_.distance()->operator()(q1, q2);
76 core::ConstraintSetPtr_t c;
77 if (constraints() && constraints()->configProjector()) {
78 c = HPP_STATIC_PTR_CAST(core::ConstraintSet, constraints()->
copy());
79 c->configProjector()->rightHandSideFromConfig(q1);
83 core::PathPtr_t path = path_t::create(problem_.robot(), q1, q2, length, c, pathDofRank_,
tds_);
91 : core::steeringMethod::Straight(*problem), pathDofRank_(pathDofRank), weak_() {}
103 : core::steeringMethod::Straight(other), pathDofRank_(other.pathDofRank_), weak_(),
tds_(other.
tds_) {}
106 void init(TimeConstraintSteeringWkPtr_t weak) {
107 core::steeringMethod::Straight::init(weak);
112 const core::PathPtr_t model_;
113 const std::size_t pathDofRank_;
114 TimeConstraintSteeringWkPtr_t weak_;
124 #endif // HPP_TIME_CONSTRAINT_STEERING_HH