hpp-rbprm
4.14.0
Implementation of RB-PRM planner using hpp.
|
#include <hpp/rbprm/rbprm-fullbody.hh>
Public Types | |
typedef std::map< std::string, std::vector< std::string > > | T_LimbGroup |
Public Member Functions | |
virtual | ~RbPrmFullBody () |
void | AddLimb (const std::string &id, const std::string &name, const std::string &effectorName, const fcl::Vec3f &offset, const fcl::Vec3f &limbOffset, const fcl::Vec3f &normal, const double x, const double y, const core::ObjectStdVector_t &collisionObjects, const std::size_t nbSamples, const std::string &heuristic="static", const double resolution=0.03, ContactType contactType=_6_DOF, const bool disableEffectorCollision=false, const bool grasp=false, const std::string &kinematicConstraintsPath=std::string(), const double kinematicConstraintsMin=0.) |
void | AddLimb (const std::string &database, const std::string &id, const core::ObjectStdVector_t &collisionObjects, const std::string &heuristicName, const bool loadValues, const bool disableEffectorCollision=false, const bool grasp=false) |
void | AddNonContactingLimb (const std::string &id, const std::string &name, const std::string &effectorName, const hpp::core::ObjectStdVector_t &collisionObjects, const std::size_t nbSamples) |
AddNonContactingLimb add a limb not used for contact generation. More... | |
bool | AddHeuristic (const std::string &name, const sampling::heuristic func) |
const rbprm::T_Limb & | GetLimbs () |
const rbprm::RbPrmLimbPtr_t | GetLimb (std::string name, bool onlyWithContact=false) |
const rbprm::T_Limb & | GetNonContactingLimbs () |
const T_LimbGroup & | GetGroups () |
const core::CollisionValidationPtr_t & | GetCollisionValidation () |
const std::map< std::string, core::CollisionValidationPtr_t > & | GetLimbCollisionValidation () |
void | staticStability (bool staticStability) |
bool | staticStability () const |
double | getFriction () const |
void | setFriction (double mu) |
pinocchio::Configuration_t | referenceConfig () |
void | referenceConfig (pinocchio::Configuration_t referenceConfig) |
pinocchio::Configuration_t | postureWeights () |
void | postureWeights (pinocchio::Configuration_t postureWeights) |
bool | usePosturalTaskContactCreation () |
void | usePosturalTaskContactCreation (bool usePosturalTaskContactCreation) |
bool | addEffectorTrajectory (const size_t pathId, const std::string &effectorName, const bezier_Ptr &trajectory) |
bool | addEffectorTrajectory (const size_t pathId, const std::string &effectorName, const std::vector< bezier_Ptr > &trajectories) |
bool | getEffectorsTrajectories (const size_t pathId, EffectorTrajectoriesMap_t &result) |
bool | getEffectorTrajectory (const size_t pathId, const std::string &effectorName, std::vector< bezier_Ptr > &result) |
bool | toggleNonContactingLimb (std::string name) |
Static Public Member Functions | |
static RbPrmFullBodyPtr_t | create (const pinocchio::DevicePtr_t &device) |
Public Attributes | |
const pinocchio::DevicePtr_t | device_ |
Protected Member Functions | |
RbPrmFullBody (const pinocchio::DevicePtr_t &device) | |
void | init (const RbPrmFullBodyWkPtr_t &weakPtr) |
Initialization. More... | |
typedef std::map<std::string, std::vector<std::string> > hpp::rbprm::RbPrmFullBody::T_LimbGroup |
|
virtual |
|
protected |
bool hpp::rbprm::RbPrmFullBody::addEffectorTrajectory | ( | const size_t | pathId, |
const std::string & | effectorName, | ||
const bezier_Ptr & | trajectory | ||
) |
bool hpp::rbprm::RbPrmFullBody::addEffectorTrajectory | ( | const size_t | pathId, |
const std::string & | effectorName, | ||
const std::vector< bezier_Ptr > & | trajectories | ||
) |
bool hpp::rbprm::RbPrmFullBody::AddHeuristic | ( | const std::string & | name, |
const sampling::heuristic | func | ||
) |
Add a new heuristic for biasing sample candidate selection
name | name used to identify the heuristic |
func | the actual heuristic method |
void hpp::rbprm::RbPrmFullBody::AddLimb | ( | const std::string & | database, |
const std::string & | id, | ||
const core::ObjectStdVector_t & | collisionObjects, | ||
const std::string & | heuristicName, | ||
const bool | loadValues, | ||
const bool | disableEffectorCollision = false , |
||
const bool | grasp = false |
||
) |
Creates a Limb for the robot, identified by its name. Stores a sample container, used for requests
database | path to the sample database used for the limbs |
id | user defined id for the limb. Must be unique. The id is used if several contact points are defined for the same limb (ex: the knee and the foot) |
collisionObjects | objects to be considered for collisions with the limb. TODO remove structure to perform efficient proximity requests. The resulution of the octree, in meters, specifies the size of the unit voxel of the octree. The larger they are, the more samples will be considered as candidates for contact. This can be problematic in terms of performance. The default value is 3 cm. |
disableEffectorCollision,whether | collision detection should be disabled for end effector bones |
void hpp::rbprm::RbPrmFullBody::AddLimb | ( | const std::string & | id, |
const std::string & | name, | ||
const std::string & | effectorName, | ||
const fcl::Vec3f & | offset, | ||
const fcl::Vec3f & | limbOffset, | ||
const fcl::Vec3f & | normal, | ||
const double | x, | ||
const double | y, | ||
const core::ObjectStdVector_t & | collisionObjects, | ||
const std::size_t | nbSamples, | ||
const std::string & | heuristic = "static" , |
||
const double | resolution = 0.03 , |
||
ContactType | contactType = _6_DOF , |
||
const bool | disableEffectorCollision = false , |
||
const bool | grasp = false , |
||
const std::string & | kinematicConstraintsPath = std::string() , |
||
const double | kinematicConstraintsMin = 0. |
||
) |
Creates a Limb for the robot, identified by its name. Stores a sample container, used for requests
id | user defined id for the limb. Must be unique. The id is used if several contact points are defined for the same limb (ex: the knee and the foot) |
name | name of the joint corresponding to the root of the limb. |
effectorName | name of the joint to be considered as the effector of the limb |
offset | position of the effector in joint coordinates relatively to the effector joint |
unit | normal vector of the contact point, expressed in the effector joint coordinates |
x | width of the default support polygon of the effector |
y | height of the default support polygon of the effector |
collisionObjects | objects to be considered for collisions with the limb. TODO remove |
nbSamples | number of samples to generate for the limb |
resolution,resolution | of the octree voxels. The samples generated are stored in an octree data structure to perform efficient proximity requests. The resulution of the octree, in meters, specifies the size of the unit voxel of the octree. The larger they are, the more samples will be considered as candidates for contact. This can be problematic in terms of performance. The default value is 3 cm. |
resolution,resolution | of the octree voxels. The samples generated are stored in an octree data |
disableEffectorCollision,whether | collision detection should be disabled for end effector bones |
void hpp::rbprm::RbPrmFullBody::AddNonContactingLimb | ( | const std::string & | id, |
const std::string & | name, | ||
const std::string & | effectorName, | ||
const hpp::core::ObjectStdVector_t & | collisionObjects, | ||
const std::size_t | nbSamples | ||
) |
AddNonContactingLimb add a limb not used for contact generation.
id | user defined id for the limb. Must be unique. The id is used if several contact points are defined for the same limb (ex: the knee and the foot) |
name | name of the joint corresponding to the root of the limb. |
effectorName | name of the joint to be considered as the effector of the limb |
collisionObjects | objects to be considered for collisions with the limb. TODO remove |
nbSamples | number of samples to generate for the limb |
|
static |
|
inline |
bool hpp::rbprm::RbPrmFullBody::getEffectorsTrajectories | ( | const size_t | pathId, |
EffectorTrajectoriesMap_t & | result | ||
) |
bool hpp::rbprm::RbPrmFullBody::getEffectorTrajectory | ( | const size_t | pathId, |
const std::string & | effectorName, | ||
std::vector< bezier_Ptr > & | result | ||
) |
|
inline |
|
inline |
const rbprm::RbPrmLimbPtr_t hpp::rbprm::RbPrmFullBody::GetLimb | ( | std::string | name, |
bool | onlyWithContact = false |
||
) |
|
inline |
|
inline |
|
inline |
|
protected |
Initialization.
|
inline |
void hpp::rbprm::RbPrmFullBody::postureWeights | ( | pinocchio::Configuration_t | postureWeights | ) |
|
inline |
void hpp::rbprm::RbPrmFullBody::referenceConfig | ( | pinocchio::Configuration_t | referenceConfig | ) |
|
inline |
|
inline |
|
inline |
bool hpp::rbprm::RbPrmFullBody::toggleNonContactingLimb | ( | std::string | name | ) |
|
inline |
|
inline |
const pinocchio::DevicePtr_t hpp::rbprm::RbPrmFullBody::device_ |