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