19#ifndef HPP_SPLINE_EFFECTOR_RRT_HH
20#define HPP_SPLINE_EFFECTOR_RRT_HH
22#include <ndcurves/bezier_curve.h>
23#include <ndcurves/curve_constraint.h>
24#include <ndcurves/exact_cubic.h>
26#include <hpp/core/config-projector.hh>
27#include <hpp/core/path.hh>
28#include <hpp/core/problem.hh>
44namespace interpolation {
45struct SetEffectorRRTConstraints;
47typedef TimeConstraintHelper<TimeConstraintPath, EffectorRRTShooterFactory,
48 SetEffectorRRTConstraints>
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);
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>());
69 const PathPtr_t comPath,
const State& startState,
const State& nextState);
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);
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>());
144 const value_type comPathLength,
const PathPtr_t fullBodyComPath,
145 const State& startState,
const State& nextState);
147typedef ndcurves::exact_cubic<double, double,
true,
148 Eigen::Matrix<value_type, 3, 1> >
150typedef ndcurves::curve_constraints<Eigen::Matrix<value_type, 3, 1> >
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)
170 const State& to)
const;
182 const PathPtr_t path,
183 const fcl::Vec3f& offset = fcl::Vec3f(0, 0, 0))
193 hppDout(notice,
"End effector path, offset = " << offset);
Definition: time-constraint-helper.hh:45
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,...
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-...
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:49
shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:12
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
Definition: effector-rrt.hh:154
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