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;
53 static RbPrmFullBodyPtr_t create (
const pinocchio::DevicePtr_t& device);
79 void AddLimb(
const std::string&
id,
const std::string& name,
const std::string& effectorName,
const fcl::Vec3f &offset,
80 const fcl::Vec3f &limbOffset,
const fcl::Vec3f &normal,
const double x,
const double y,
81 const core::ObjectStdVector_t &collisionObjects,
82 const std::size_t nbSamples,
const std::string&
heuristic =
"static",
const double resolution = 0.03,
84 const bool grasp =
false,
const std::string &kinematicConstraintsPath=std::string(),
const double kinematicConstraintsMin=0.);
98 void AddLimb(
const std::string& database,
const std::string&
id,
const core::ObjectStdVector_t &collisionObjects,
99 const std::string& heuristicName,
const bool loadValues,
const bool disableEffectorCollision =
false,
100 const bool grasp =
false);
110 void AddNonContactingLimb(
const std::string&
id,
const std::string& name,
const std::string &effectorName,
111 const hpp::core::ObjectStdVector_t &collisionObjects,
const std::size_t nbSamples);
121 typedef std::map<std::string, std::vector<std::string> >
T_LimbGroup;
136 void referenceConfig(pinocchio::Configuration_t referenceConfig);
138 void postureWeights(pinocchio::Configuration_t postureWeights);
141 bool addEffectorTrajectory(
const size_t pathId,
const std::string& effectorName,
const bezier_Ptr& trajectory);
142 bool addEffectorTrajectory(
const size_t pathId,
const std::string& effectorName,
const std::vector<bezier_Ptr>& trajectories);
143 bool getEffectorsTrajectories(
const size_t pathId,EffectorTrajectoriesMap_t& result);
144 bool getEffectorTrajectory(
const size_t pathId,
const std::string& effectorName,std::vector<bezier_Ptr>& result);
145 bool toggleNonContactingLimb(std::string name);
147 core::CollisionValidationPtr_t collisionValidation_;
148 std::map<std::string, core::CollisionValidationPtr_t> limbcollisionValidations_;
151 T_LimbGroup limbGroups_;
153 bool staticStability_;
155 pinocchio::Configuration_t reference_;
156 pinocchio::Configuration_t postureWeights_;
157 bool usePosturalTaskContactCreation_;
158 std::map<size_t,EffectorTrajectoriesMap_t> effectorsTrajectoriesMaps_;
161 const hpp::core::ObjectStdVector_t &collisionObjects,
const bool disableEffectorCollision,
const bool nonContactingLimb=
false);
169 void init (
const RbPrmFullBodyWkPtr_t& weakPtr);
172 RbPrmFullBodyWkPtr_t weakPtr_;
178 #endif // HPP_RBPRM_DEVICE_HH hpp::core::Container< hpp::core::AffordanceObjects_t > affMap_t
Definition: rbprm-fullbody.hh:47
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64
ContactType
Definition: rbprm-limb.hh:30
boost::shared_ptr< bezier_t > bezier_Ptr
Definition: bezier-path.hh:32
Definition: algorithm.hh:27
Definition: rbprm-limb.hh:32
const rbprm::T_Limb & GetNonContactingLimbs()
Definition: rbprm-fullbody.hh:126
double(* heuristic)(const sampling::Sample &sample, const Eigen::Vector3d &direction, const Eigen::Vector3d &normal, const HeuristicParam ¶ms)
Definition: heuristic.hh:41
const core::CollisionValidationPtr_t & GetCollisionValidation()
Definition: rbprm-fullbody.hh:128
std::map< std::string, std::vector< std::string > > T_LimbGroup
Definition: rbprm-fullbody.hh:121
void staticStability(bool staticStability)
Definition: rbprm-fullbody.hh:131
void usePosturalTaskContactCreation(bool usePosturalTaskContactCreation)
Definition: rbprm-fullbody.hh:140
boost::shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:12
HPP_PREDEF_CLASS(RbPrmFullBody)
bool staticStability() const
Definition: rbprm-fullbody.hh:132
std::map< std::string, std::vector< bezier_Ptr > > EffectorTrajectoriesMap_t
Definition: rbprm-fullbody.hh:48
double getFriction() const
Definition: rbprm-fullbody.hh:133
Definition: rbprm-fullbody.hh:50
pinocchio::Configuration_t postureWeights()
Definition: rbprm-fullbody.hh:137
boost::shared_ptr< RbPrmLimb > RbPrmLimbPtr_t
Definition: rbprm-limb.hh:43
void setFriction(double mu)
Definition: rbprm-fullbody.hh:134
const T_LimbGroup & GetGroups()
Definition: rbprm-fullbody.hh:127
std::map< std::string, const rbprm::RbPrmLimbPtr_t > T_Limb
Definition: rbprm-limb.hh:45
Definition: heuristic.hh:47
const pinocchio::DevicePtr_t device_
Definition: rbprm-fullbody.hh:130
const rbprm::T_Limb & GetLimbs()
Definition: rbprm-fullbody.hh:124
pinocchio::Configuration_t referenceConfig()
Definition: rbprm-fullbody.hh:135
bool usePosturalTaskContactCreation()
Definition: rbprm-fullbody.hh:139
const std::map< std::string, core::CollisionValidationPtr_t > & GetLimbCollisionValidation()
Definition: rbprm-fullbody.hh:129