19 #ifndef HPP_RBPRM_FULLBODY_HH
20 #define HPP_RBPRM_FULLBODY_HH
24 #include <hpp/pinocchio/device.hh>
26 #include <hpp/core/collision-validation.hh>
27 #include <hpp/core/problem-solver.hh>
36 using core::size_type;
47 typedef hpp::core::Container<hpp::core::AffordanceObjects_t>
affMap_t;
78 void AddLimb(
const std::string&
id,
const std::string& name,
const std::string& effectorName,
79 const fcl::Vec3f& offset,
const fcl::Vec3f& limbOffset,
const fcl::Vec3f& normal,
const double x,
80 const double y,
const core::ObjectStdVector_t& collisionObjects,
const std::size_t nbSamples,
81 const std::string&
heuristic =
"static",
const double resolution = 0.03,
82 ContactType contactType =
_6_DOF,
const bool disableEffectorCollision =
false,
const bool grasp =
false,
83 const std::string& kinematicConstraintsPath = std::string(),
const double kinematicConstraintsMin = 0.);
97 void AddLimb(
const std::string& database,
const std::string&
id,
const core::ObjectStdVector_t& collisionObjects,
98 const std::string& heuristicName,
const bool loadValues,
const bool disableEffectorCollision =
false,
99 const bool grasp =
false);
109 void AddNonContactingLimb(
const std::string&
id,
const std::string& name,
const std::string& effectorName,
110 const hpp::core::ObjectStdVector_t& collisionObjects,
const std::size_t nbSamples);
120 typedef std::map<std::string, std::vector<std::string> >
T_LimbGroup;
129 return limbcollisionValidations_;
137 void referenceConfig(pinocchio::Configuration_t referenceConfig);
139 void postureWeights(pinocchio::Configuration_t postureWeights);
142 usePosturalTaskContactCreation_ = usePosturalTaskContactCreation;
144 bool addEffectorTrajectory(
const size_t pathId,
const std::string& effectorName,
const bezier_Ptr& trajectory);
145 bool addEffectorTrajectory(
const size_t pathId,
const std::string& effectorName,
146 const std::vector<bezier_Ptr>& trajectories);
148 bool getEffectorTrajectory(
const size_t pathId,
const std::string& effectorName, std::vector<bezier_Ptr>& result);
149 bool toggleNonContactingLimb(std::string name);
152 core::CollisionValidationPtr_t collisionValidation_;
153 std::map<std::string, core::CollisionValidationPtr_t> limbcollisionValidations_;
156 T_LimbGroup limbGroups_;
158 bool staticStability_;
160 pinocchio::Configuration_t reference_;
161 pinocchio::Configuration_t postureWeights_;
162 bool usePosturalTaskContactCreation_;
164 std::map<size_t, EffectorTrajectoriesMap_t>
165 effectorsTrajectoriesMaps_;
169 const hpp::core::ObjectStdVector_t& collisionObjects,
const bool disableEffectorCollision,
170 const bool nonContactingLimb =
false);
178 void init(
const RbPrmFullBodyWkPtr_t& weakPtr);
181 RbPrmFullBodyWkPtr_t weakPtr_;
187 #endif // HPP_RBPRM_DEVICE_HH