hpp-rbprm  4.15.1
Implementation of RB-PRM planner using hpp.
effector-rrt.hh
Go to the documentation of this file.
1 // This file is part of hpp-wholebody-step.
6 // hpp-wholebody-step-planner 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-wholebody-step-planner 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-wholebody-step-planner. If not, see
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef HPP_SPLINE_EFFECTOR_RRT_HH
20 #define HPP_SPLINE_EFFECTOR_RRT_HH
21 
22 #include <ndcurves/bezier_curve.h>
23 #include <ndcurves/curve_constraint.h>
24 #include <ndcurves/exact_cubic.h>
25 
26 #include <hpp/core/config-projector.hh>
27 #include <hpp/core/path.hh>
28 #include <hpp/core/problem.hh>
29 #include <hpp/rbprm/config.hh>
38 #include <hpp/rbprm/rbprm-state.hh>
39 #include <map>
40 #include <vector>
41 
42 namespace hpp {
43 namespace rbprm {
44 namespace interpolation {
45 struct SetEffectorRRTConstraints;
46 
47 typedef TimeConstraintHelper<TimeConstraintPath, EffectorRRTShooterFactory,
48  SetEffectorRRTConstraints>
50 
51 core::PathPtr_t effectorRRT(RbPrmFullBodyPtr_t fullbody,
52  core::ProblemSolverPtr_t problemSolver,
53  const PathPtr_t comPath, const State& startState,
54  const State& nextState,
55  const std::size_t numOptimizations,
56  const bool keepExtraDof);
57 
58 core::PathPtr_t effectorRRT(
59  RbPrmFullBodyPtr_t fullbody, core::ProblemSolverPtr_t problemSolver,
60  const PathPtr_t comPath, const State& startState, const State& nextState,
61  const std::size_t numOptimizations, const bool keepExtraDof,
62  const std::vector<std::string>& constrainedJointPos =
63  std::vector<std::string>(),
64  const std::vector<std::string>& constrainedLockedJoints =
65  std::vector<std::string>());
66 
67 core::PathPtr_t generateEndEffectorBezier(
68  RbPrmFullBodyPtr_t fullbody, core::ProblemSolverPtr_t problemSolver,
69  const PathPtr_t comPath, const State& startState, const State& nextState);
70 
93  RbPrmFullBodyPtr_t fullbody, core::ProblemSolverPtr_t problemSolver,
94  const PathPtr_t comPath, const PathPtr_t fullBodyComPath,
95  const State& startState, const State& nextState,
96  const std::size_t numOptimizations, const bool keepExtraDof,
97  const PathPtr_t refPath,
98  const std::vector<std::string>& constrainedJointPos,
99  const std::vector<std::string>& constrainedLockedJoints);
100 
121 core::PathPtr_t effectorRRTFromPath(
122  RbPrmFullBodyPtr_t fullbody, core::ProblemSolverPtr_t problemSolver,
123  const PathPtr_t comPath, const State& startState, const State& nextState,
124  const std::size_t numOptimizations, const bool keepExtraDof,
125  const PathPtr_t refFullBodyPath,
126  const std::vector<std::string>& constrainedJointPos =
127  std::vector<std::string>(),
128  const std::vector<std::string>& constrainedLockedJoints =
129  std::vector<std::string>());
130 
142 std::vector<core::PathVectorPtr_t> fitBeziersToPath(
143  RbPrmFullBodyPtr_t fullbody, const pinocchio::Frame& effector,
144  const value_type comPathLength, const PathPtr_t fullBodyComPath,
145  const State& startState, const State& nextState);
146 
147 typedef ndcurves::exact_cubic<double, double, true,
148  Eigen::Matrix<value_type, 3, 1> >
150 typedef ndcurves::curve_constraints<Eigen::Matrix<value_type, 3, 1> >
152 typedef shared_ptr<exact_cubic_t> exact_cubic_Ptr;
153 
156  const core::PathPtr_t refCom, const core::PathPtr_t refEff,
157  const core::PathPtr_t refFullbody, const pinocchio::Frame effector,
158  const pinocchio::DevicePtr_t endEffectorDevice,
159  const std::vector<pinocchio::JointPtr_t>& constrainedJointPos,
160  const std::vector<pinocchio::JointPtr_t>& constrainedLockedJoints)
161  : refCom_(refCom),
162  refFullbody_(refFullbody),
163  refEff_(refEff),
164  effector_(effector),
165  endEffectorDevice_(endEffectorDevice),
166  constrainedJointPos_(constrainedJointPos),
167  constrainedLockedJoints_(constrainedLockedJoints) {}
168 
169  void operator()(EffectorRRTHelper& helper, const State& from,
170  const State& to) const;
171  const core::PathPtr_t refCom_;
172  const core::PathPtr_t refFullbody_;
173  const core::PathPtr_t refEff_;
174  const pinocchio::Frame effector_;
175  const pinocchio::DevicePtr_t endEffectorDevice_;
176  const std::vector<pinocchio::JointPtr_t> constrainedJointPos_;
177  const std::vector<pinocchio::JointPtr_t> constrainedLockedJoints_;
178 };
179 
181  EndEffectorPath(const DevicePtr_t device, const pinocchio::Frame& effector,
182  const PathPtr_t path,
183  const fcl::Vec3f& offset = fcl::Vec3f(0, 0, 0))
184  : device_(device),
185  effector_(effector),
186  fullBodyPath_(path),
188  createPositionMethod(device, fcl::Vec3f(), effector)),
189  offset_(offset),
190  length_(path->length()) {}
191  vector_t operator()(value_type t) const;
192  void setOffset(const fcl::Vec3f& offset) {
193  hppDout(notice, "End effector path, offset = " << offset);
194  offset_ = offset;
195  }
196 
197  const core::DevicePtr_t device_;
198  const pinocchio::Frame effector_;
199  const core::PathPtr_t fullBodyPath_;
200  constraints::PositionPtr_t positionConstraint_;
201  fcl::Vec3f offset_;
202  const value_type length_;
203 };
204 
205 } // namespace interpolation
206 } // namespace rbprm
207 } // namespace hpp
208 #endif // HPP_SPLINE_EFFECTOR_TRAJECTORY_HH
Definition: time-constraint-helper.hh:45
shared_ptr< exact_cubic_t > exact_cubic_Ptr
Definition: effector-rrt.hh:152
ndcurves::curve_constraints< Eigen::Matrix< value_type, 3, 1 > > curve_constraint_t
Definition: effector-rrt.hh:151
ndcurves::exact_cubic< double, double, true, Eigen::Matrix< value_type, 3, 1 > > exact_cubic_t
Definition: effector-rrt.hh:149
core::PathPtr_t generateEndEffectorBezier(RbPrmFullBodyPtr_t fullbody, core::ProblemSolverPtr_t problemSolver, const PathPtr_t comPath, const State &startState, const State &nextState)
core::PathPtr_t effectorRRT(RbPrmFullBodyPtr_t fullbody, core::ProblemSolverPtr_t problemSolver, const PathPtr_t comPath, const State &startState, const State &nextState, const std::size_t numOptimizations, const bool keepExtraDof)
PathPtr_t effectorRRTFromPath(RbPrmFullBodyPtr_t fullbody, core::ProblemSolverPtr_t problemSolver, const PathPtr_t comPath, const PathPtr_t fullBodyComPath, const State &startState, const State &nextState, const std::size_t numOptimizations, const bool keepExtraDof, const PathPtr_t refPath, const std::vector< std::string > &constrainedJointPos, const std::vector< std::string > &constrainedLockedJoints)
effectorRRTFromPath Call comRRT to compute a whole body path between two states, then compute an end-...
std::vector< core::PathVectorPtr_t > fitBeziersToPath(RbPrmFullBodyPtr_t fullbody, const pinocchio::Frame &effector, const value_type comPathLength, const PathPtr_t fullBodyComPath, const State &startState, const State &nextState)
fitBeziersToPath generate a vector of pathVector : each pathVector containt BezierPath,...
constraints::PositionPtr_t createPositionMethod(pinocchio::DevicePtr_t device, const fcl::Vec3f &initTarget, const pinocchio::Frame effectorFrame)
Definition: interpolation-constraints.hh:219
TimeConstraintHelper< TimeConstraintPath, EffectorRRTShooterFactory, SetEffectorRRTConstraints > EffectorRRTHelper
Definition: effector-rrt.hh:45
shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:11
Definition: algorithm.hh:26
Definition: rbprm-state.hh:40
Definition: effector-rrt.hh:180
const core::PathPtr_t fullBodyPath_
Definition: effector-rrt.hh:199
EndEffectorPath(const DevicePtr_t device, const pinocchio::Frame &effector, const PathPtr_t path, const fcl::Vec3f &offset=fcl::Vec3f(0, 0, 0))
Definition: effector-rrt.hh:181
const pinocchio::Frame effector_
Definition: effector-rrt.hh:198
fcl::Vec3f offset_
Definition: effector-rrt.hh:201
vector_t operator()(value_type t) const
const value_type length_
Definition: effector-rrt.hh:202
const core::DevicePtr_t device_
Definition: effector-rrt.hh:197
constraints::PositionPtr_t positionConstraint_
Definition: effector-rrt.hh:200
void setOffset(const fcl::Vec3f &offset)
Definition: effector-rrt.hh:192
const std::vector< pinocchio::JointPtr_t > constrainedLockedJoints_
Definition: effector-rrt.hh:177
const core::PathPtr_t refEff_
Definition: effector-rrt.hh:173
const core::PathPtr_t refFullbody_
Definition: effector-rrt.hh:172
const core::PathPtr_t refCom_
Definition: effector-rrt.hh:171
const pinocchio::DevicePtr_t endEffectorDevice_
Definition: effector-rrt.hh:175
const pinocchio::Frame effector_
Definition: effector-rrt.hh:174
SetEffectorRRTConstraints(const core::PathPtr_t refCom, const core::PathPtr_t refEff, const core::PathPtr_t refFullbody, const pinocchio::Frame effector, const pinocchio::DevicePtr_t endEffectorDevice, const std::vector< pinocchio::JointPtr_t > &constrainedJointPos, const std::vector< pinocchio::JointPtr_t > &constrainedLockedJoints)
Definition: effector-rrt.hh:155
const std::vector< pinocchio::JointPtr_t > constrainedJointPos_
Definition: effector-rrt.hh:176
void operator()(EffectorRRTHelper &helper, const State &from, const State &to) const