hpp-rbprm  4.11.0
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 <hpp/rbprm/config.hh>
24 #include <hpp/rbprm/rbprm-state.hh>
31 #include <hpp/core/path.hh>
32 #include <hpp/core/problem.hh>
33 #include <hpp/core/config-projector.hh>
35 #include <ndcurves/exact_cubic.h>
36 #include <ndcurves/bezier_curve.h>
37 #include <ndcurves/curve_constraint.h>
38 #include <vector>
39 #include <map>
40 
41 namespace hpp {
42 namespace rbprm {
43 namespace interpolation {
45 
48 
49 core::PathPtr_t effectorRRT(RbPrmFullBodyPtr_t fullbody, core::ProblemSolverPtr_t problemSolver,
50  const PathPtr_t comPath, const State& startState, const State& nextState,
51  const std::size_t numOptimizations, const bool keepExtraDof);
52 
53 core::PathPtr_t effectorRRT(RbPrmFullBodyPtr_t fullbody, core::ProblemSolverPtr_t problemSolver,
54  const PathPtr_t comPath, const State& startState, const State& nextState,
55  const std::size_t numOptimizations, const bool keepExtraDof,
56  const std::vector<std::string>& constrainedJointPos = std::vector<std::string>(),
57  const std::vector<std::string>& constrainedLockedJoints = std::vector<std::string>());
58 
59 core::PathPtr_t generateEndEffectorBezier(RbPrmFullBodyPtr_t fullbody, core::ProblemSolverPtr_t problemSolver,
60  const PathPtr_t comPath, const State& startState, const State& nextState);
61 
81 PathPtr_t effectorRRTFromPath(RbPrmFullBodyPtr_t fullbody, core::ProblemSolverPtr_t problemSolver,
82  const PathPtr_t comPath, const PathPtr_t fullBodyComPath, const State& startState,
83  const State& nextState, const std::size_t numOptimizations, const bool keepExtraDof,
84  const PathPtr_t refPath, const std::vector<std::string>& constrainedJointPos,
85  const std::vector<std::string>& constrainedLockedJoints);
86 
105 core::PathPtr_t effectorRRTFromPath(
106  RbPrmFullBodyPtr_t fullbody, core::ProblemSolverPtr_t problemSolver, const PathPtr_t comPath,
107  const State& startState, const State& nextState, const std::size_t numOptimizations, const bool keepExtraDof,
108  const PathPtr_t refFullBodyPath, const std::vector<std::string>& constrainedJointPos = std::vector<std::string>(),
109  const std::vector<std::string>& constrainedLockedJoints = std::vector<std::string>());
110 
122 std::vector<core::PathVectorPtr_t> fitBeziersToPath(RbPrmFullBodyPtr_t fullbody, const pinocchio::Frame& effector,
123  const value_type comPathLength, const PathPtr_t fullBodyComPath,
124  const State& startState, const State& nextState);
125 
126 typedef ndcurves::exact_cubic<double, double, true, Eigen::Matrix<value_type, 3, 1> > exact_cubic_t;
127 typedef ndcurves::curve_constraints<Eigen::Matrix<value_type, 3, 1> > curve_constraint_t;
128 typedef std::shared_ptr<exact_cubic_t> exact_cubic_Ptr;
129 
131  SetEffectorRRTConstraints(const core::PathPtr_t refCom, const core::PathPtr_t refEff,
132  const core::PathPtr_t refFullbody, const pinocchio::Frame effector,
133  const pinocchio::DevicePtr_t endEffectorDevice,
134  const std::vector<pinocchio::JointPtr_t>& constrainedJointPos,
135  const std::vector<pinocchio::JointPtr_t>& constrainedLockedJoints)
136  : refCom_(refCom),
137  refFullbody_(refFullbody),
138  refEff_(refEff),
139  effector_(effector),
140  endEffectorDevice_(endEffectorDevice),
141  constrainedJointPos_(constrainedJointPos),
142  constrainedLockedJoints_(constrainedLockedJoints) {}
143 
144  void operator()(EffectorRRTHelper& helper, const State& from, const State& to) const;
145  const core::PathPtr_t refCom_;
146  const core::PathPtr_t refFullbody_;
147  const core::PathPtr_t refEff_;
148  const pinocchio::Frame effector_;
149  const pinocchio::DevicePtr_t endEffectorDevice_;
150  const std::vector<pinocchio::JointPtr_t> constrainedJointPos_;
151  const std::vector<pinocchio::JointPtr_t> constrainedLockedJoints_;
152 };
153 
155  EndEffectorPath(const DevicePtr_t device, const pinocchio::Frame& effector, const PathPtr_t path,
156  const fcl::Vec3f& offset = fcl::Vec3f(0, 0, 0))
157  : device_(device),
158  effector_(effector),
159  fullBodyPath_(path),
160  positionConstraint_(createPositionMethod(device, fcl::Vec3f(), effector)),
161  offset_(offset),
162  length_(path->length()) {}
163  vector_t operator()(value_type t) const;
164  void setOffset(const fcl::Vec3f& offset) {
165  hppDout(notice, "End effector path, offset = " << offset);
166  offset_ = offset;
167  }
168 
169  const core::DevicePtr_t device_;
170  const pinocchio::Frame effector_;
171  const core::PathPtr_t fullBodyPath_;
172  constraints::PositionPtr_t positionConstraint_;
173  fcl::Vec3f offset_;
174  const value_type length_;
175 };
176 
177 } // namespace interpolation
178 } // namespace rbprm
179 } // namespace hpp
180 #endif // HPP_SPLINE_EFFECTOR_TRAJECTORY_HH
const pinocchio::Frame effector_
Definition: effector-rrt.hh:170
std::shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:12
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:131
const pinocchio::Frame effector_
Definition: effector-rrt.hh:148
const core::PathPtr_t refEff_
Definition: effector-rrt.hh:147
Definition: algorithm.hh:27
const core::PathPtr_t refFullbody_
Definition: effector-rrt.hh:146
Definition: effector-rrt.hh:154
const std::vector< pinocchio::JointPtr_t > constrainedJointPos_
Definition: effector-rrt.hh:150
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)
constraints::PositionPtr_t createPositionMethod(pinocchio::DevicePtr_t device, const fcl::Vec3f &initTarget, const pinocchio::Frame effectorFrame)
Definition: interpolation-constraints.hh:202
ndcurves::exact_cubic< double, double, true, Eigen::Matrix< value_type, 3, 1 > > exact_cubic_t
Definition: effector-rrt.hh:126
core::PathPtr_t generateEndEffectorBezier(RbPrmFullBodyPtr_t fullbody, core::ProblemSolverPtr_t problemSolver, const PathPtr_t comPath, const State &startState, const State &nextState)
ndcurves::curve_constraints< Eigen::Matrix< value_type, 3, 1 > > curve_constraint_t
Definition: effector-rrt.hh:127
const std::vector< pinocchio::JointPtr_t > constrainedLockedJoints_
Definition: effector-rrt.hh:151
const core::PathPtr_t refCom_
Definition: effector-rrt.hh:145
TimeConstraintHelper< TimeConstraintPath, EffectorRRTShooterFactory, SetEffectorRRTConstraints > EffectorRRTHelper
Definition: effector-rrt.hh:44
const core::DevicePtr_t device_
Definition: effector-rrt.hh:169
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, computed with varying value of weight-rrt
fcl::Vec3f offset_
Definition: effector-rrt.hh:173
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:155
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-...
void operator()(EffectorRRTHelper &helper, const State &from, const State &to) const
const value_type length_
Definition: effector-rrt.hh:174
std::shared_ptr< exact_cubic_t > exact_cubic_Ptr
Definition: effector-rrt.hh:128
void setOffset(const fcl::Vec3f &offset)
Definition: effector-rrt.hh:164
Definition: rbprm-state.hh:40
const core::PathPtr_t fullBodyPath_
Definition: effector-rrt.hh:171
constraints::PositionPtr_t positionConstraint_
Definition: effector-rrt.hh:172
const pinocchio::DevicePtr_t endEffectorDevice_
Definition: effector-rrt.hh:149
Definition: time-constraint-helper.hh:46