hpp-rbprm 4.15.1
Implementation of RB-PRM planner using hpp.
Loading...
Searching...
No Matches
time-constraint-helper.hh
Go to the documentation of this file.
1//
2// Copyright (c) 2014 CNRS
3// Authors: Steve Tonneau (steve.tonneau@laas.fr)
4//
5// This file is part of hpp-rbprm.
6// hpp-rbprm 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-rbprm 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_HELPER_HH
20#define HPP_TIME_CONSTRAINT_HELPER_HH
21
22#include <hpp/core/config-projector.hh>
23#include <hpp/core/path-projector/progressive.hh>
24#include <hpp/core/path.hh>
25#include <hpp/core/problem.hh>
26#include <hpp/core/weighed-distance.hh>
27#include <hpp/rbprm/config.hh>
32#include <map>
33#include <vector>
34
35namespace hpp {
36namespace rbprm {
37namespace interpolation {
38
44template <class Path_T, class ShooterFactory_T, typename ConstraintFactory_T>
45class HPP_CORE_DLLAPI TimeConstraintHelper {
46 typedef shared_ptr<core::pathProjector::Progressive> ProgressivePtr_t;
47 typedef core::pathProjector::Progressive Progressive;
48
49 public:
56 const ShooterFactory_T& shooterFactory,
57 const ConstraintFactory_T& constraintFactory,
58 core::ProblemPtr_t referenceProblem,
59 core::PathPtr_t refPath,
60 const pinocchio::value_type error_treshold = 1e-3)
61 : fullbody_(fullbody),
62 fullBodyDevice_(fullbody->device_->clone()),
63 rootProblem_(core::Problem::create(fullBodyDevice_)),
64 refPath_(refPath),
65 shooterFactory_(shooterFactory),
66 constraintFactory_(constraintFactory) {
67 // adding extra DOF for including time in sampling
68 fullBodyDevice_->setDimensionExtraConfigSpace(
69 fullBodyDevice_->extraConfigSpace().dimension() + 1);
70 proj_ = core::ConfigProjector::create(rootProblem_->robot(), "proj",
71 error_treshold, 1000);
72 rootProblem_->collisionObstacles(referenceProblem->collisionObstacles());
74 rootProblem_, fullBodyDevice_->configSize() - 1);
75 rootProblem_->steeringMethod(steeringMethod_);
76 ProgressivePtr_t pProj =
77 Progressive::create(rootProblem_->distance(), steeringMethod_, 0.06);
78 // rootProblem_->pathProjector(pProj);
79 }
80
82
83 void SetConstraints(const State& from, const State& to) {
84 constraintFactory_(*this, from, to);
85 }
86 void SetConfigShooter(const State& from, const State& to);
88 void SetContactConstraints(const State& from, const State& to);
89 core::PathVectorPtr_t Run(const State& from, const State& to,
90 const size_t maxIterations = 0);
91
92 public:
94 core::DevicePtr_t fullBodyDevice_;
95 core::ProblemPtr_t rootProblem_;
96 core::PathPlannerPtr_t planner_;
97 core::PathPtr_t refPath_;
98 core::ConfigProjectorPtr_t proj_;
99 shared_ptr<TimeConstraintSteering<Path_T> > steeringMethod_;
100 const ShooterFactory_T& shooterFactory_;
101 const ConstraintFactory_T& constraintFactory_;
102};
103
134template <class Helper_T, class ShooterFactory_T, typename ConstraintFactory_T,
135 typename StateConstIterator>
137 RbPrmFullBodyPtr_t fullbody, core::ProblemPtr_t referenceProblem,
138 const ShooterFactory_T& shooterFactory,
139 const ConstraintFactory_T& constraintFactory,
140 const StateConstIterator& startState, const StateConstIterator& endState,
141 const std::size_t numOptimizations = 10, const bool keepExtraDof = false,
142 const pinocchio::value_type error_treshold = 0.001,
143 const size_t maxIterations = 0);
144
174template <class Helper_T, class ShooterFactory_T, typename ConstraintFactory_T>
176 RbPrmFullBodyPtr_t fullbody, core::ProblemPtr_t referenceProblem,
177 const ShooterFactory_T& shooterFactory,
178 const ConstraintFactory_T& constraintFactory, const core::PathPtr_t refPath,
179 const CIT_StateFrame& startState, const CIT_StateFrame& endState,
180 const std::size_t numOptimizations = 10, const bool keepExtraDof = false,
181 const pinocchio::value_type error_treshold = 0.001,
182 const size_t maxIterations = 0);
183
184/*typedef core::PathPtr_t (*interpolate_states)(rbprm::RbPrmFullBodyPtr_t,
185core::ProblemPtr_t,const rbprm::CIT_State&, const rbprm::CIT_State&,const
186std::size_t);
187
188typedef core::PathPtr_t
189(*interpolate_states_from_path)(rbprm::RbPrmFullBodyPtr_t, core::ProblemPtr_t,
190const core::PathPtr_t, const rbprm::CIT_StateFrame&,const
191rbprm::CIT_StateFrame&,const std::size_t);*/
192} // namespace interpolation
193} // namespace rbprm
194} // namespace hpp
195
196#include <hpp/rbprm/interpolation/time-constraint-helper.inl>
197#endif // HPP_TIME_CONSTRAINT_HELPER_HH
Definition: time-constraint-helper.hh:45
const ShooterFactory_T & shooterFactory_
Definition: time-constraint-helper.hh:100
core::ProblemPtr_t rootProblem_
Definition: time-constraint-helper.hh:95
const ConstraintFactory_T & constraintFactory_
Definition: time-constraint-helper.hh:101
core::DevicePtr_t fullBodyDevice_
Definition: time-constraint-helper.hh:94
TimeConstraintHelper(RbPrmFullBodyPtr_t fullbody, const ShooterFactory_T &shooterFactory, const ConstraintFactory_T &constraintFactory, core::ProblemPtr_t referenceProblem, core::PathPtr_t refPath, const pinocchio::value_type error_treshold=1e-3)
Definition: time-constraint-helper.hh:55
void SetConfigShooter(const State &from, const State &to)
~TimeConstraintHelper()
Definition: time-constraint-helper.hh:81
core::PathPlannerPtr_t planner_
Definition: time-constraint-helper.hh:96
shared_ptr< TimeConstraintSteering< Path_T > > steeringMethod_
Definition: time-constraint-helper.hh:99
void SetContactConstraints(const State &from, const State &to)
core::PathPtr_t refPath_
Definition: time-constraint-helper.hh:97
core::PathVectorPtr_t Run(const State &from, const State &to, const size_t maxIterations=0)
RbPrmFullBodyPtr_t fullbody_
Definition: time-constraint-helper.hh:93
void SetConstraints(const State &from, const State &to)
Definition: time-constraint-helper.hh:83
core::ConfigProjectorPtr_t proj_
Definition: time-constraint-helper.hh:98
Definition: time-constraint-steering.hh:41
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64
core::PathPtr_t HPP_RBPRM_DLLAPI interpolateStates(RbPrmFullBodyPtr_t fullbody, core::ProblemPtr_t referenceProblem, const ShooterFactory_T &shooterFactory, const ConstraintFactory_T &constraintFactory, const StateConstIterator &startState, const StateConstIterator &endState, const std::size_t numOptimizations=10, const bool keepExtraDof=false, const pinocchio::value_type error_treshold=0.001, const size_t maxIterations=0)
core::PathPtr_t HPP_RBPRM_DLLAPI interpolateStatesFromPath(RbPrmFullBodyPtr_t fullbody, core::ProblemPtr_t referenceProblem, const ShooterFactory_T &shooterFactory, const ConstraintFactory_T &constraintFactory, const core::PathPtr_t refPath, const CIT_StateFrame &startState, const CIT_StateFrame &endState, const std::size_t numOptimizations=10, const bool keepExtraDof=false, const pinocchio::value_type error_treshold=0.001, const size_t maxIterations=0)
shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:12
T_StateFrame::const_iterator CIT_StateFrame
Definition: rbprm-state.hh:35
Definition: algorithm.hh:26
Definition: rbprm-state.hh:40