hpp-rbprm  4.15.1
Implementation of RB-PRM planner using hpp.
hpp::rbprm::RbPrmFullBody Class Reference

#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_LimbGetLimbs ()
 
const rbprm::RbPrmLimbPtr_t GetLimb (std::string name, bool onlyWithContact=false)
 
const rbprm::T_LimbGetNonContactingLimbs ()
 
const T_LimbGroupGetGroups ()
 
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...
 

Member Typedef Documentation

◆ T_LimbGroup

typedef std::map<std::string, std::vector<std::string> > hpp::rbprm::RbPrmFullBody::T_LimbGroup

Constructor & Destructor Documentation

◆ ~RbPrmFullBody()

virtual hpp::rbprm::RbPrmFullBody::~RbPrmFullBody ( )
virtual

◆ RbPrmFullBody()

hpp::rbprm::RbPrmFullBody::RbPrmFullBody ( const pinocchio::DevicePtr_t &  device)
protected

Member Function Documentation

◆ addEffectorTrajectory() [1/2]

bool hpp::rbprm::RbPrmFullBody::addEffectorTrajectory ( const size_t  pathId,
const std::string &  effectorName,
const bezier_Ptr trajectory 
)

◆ addEffectorTrajectory() [2/2]

bool hpp::rbprm::RbPrmFullBody::addEffectorTrajectory ( const size_t  pathId,
const std::string &  effectorName,
const std::vector< bezier_Ptr > &  trajectories 
)

◆ AddHeuristic()

bool hpp::rbprm::RbPrmFullBody::AddHeuristic ( const std::string &  name,
const sampling::heuristic  func 
)

Add a new heuristic for biasing sample candidate selection

Parameters
namename used to identify the heuristic
functhe actual heuristic method
Returns
whether the heuristic has been added. False is returned if a heuristic with that name already exists.

◆ AddLimb() [1/2]

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

Parameters
databasepath to the sample database used for the limbs
iduser 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)
collisionObjectsobjects 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,whethercollision detection should be disabled for end effector bones

◆ AddLimb() [2/2]

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

Parameters
iduser 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)
namename of the joint corresponding to the root of the limb.
effectorNamename of the joint to be considered as the effector of the limb
offsetposition of the effector in joint coordinates relatively to the effector joint
unitnormal vector of the contact point, expressed in the effector joint coordinates
xwidth of the default support polygon of the effector
yheight of the default support polygon of the effector
collisionObjectsobjects to be considered for collisions with the limb. TODO remove
nbSamplesnumber of samples to generate for the limb
resolution,resolutionof 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,resolutionof the octree voxels. The samples generated are stored in an octree data
disableEffectorCollision,whethercollision detection should be disabled for end effector bones

◆ AddNonContactingLimb()

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.

Parameters
iduser 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)
namename of the joint corresponding to the root of the limb.
effectorNamename of the joint to be considered as the effector of the limb
collisionObjectsobjects to be considered for collisions with the limb. TODO remove
nbSamplesnumber of samples to generate for the limb

◆ create()

static RbPrmFullBodyPtr_t hpp::rbprm::RbPrmFullBody::create ( const pinocchio::DevicePtr_t &  device)
static

◆ GetCollisionValidation()

const core::CollisionValidationPtr_t& hpp::rbprm::RbPrmFullBody::GetCollisionValidation ( )
inline

◆ getEffectorsTrajectories()

bool hpp::rbprm::RbPrmFullBody::getEffectorsTrajectories ( const size_t  pathId,
EffectorTrajectoriesMap_t result 
)

◆ getEffectorTrajectory()

bool hpp::rbprm::RbPrmFullBody::getEffectorTrajectory ( const size_t  pathId,
const std::string &  effectorName,
std::vector< bezier_Ptr > &  result 
)

◆ getFriction()

double hpp::rbprm::RbPrmFullBody::getFriction ( ) const
inline

◆ GetGroups()

const T_LimbGroup& hpp::rbprm::RbPrmFullBody::GetGroups ( )
inline

◆ GetLimb()

const rbprm::RbPrmLimbPtr_t hpp::rbprm::RbPrmFullBody::GetLimb ( std::string  name,
bool  onlyWithContact = false 
)

◆ GetLimbCollisionValidation()

const std::map<std::string, core::CollisionValidationPtr_t>& hpp::rbprm::RbPrmFullBody::GetLimbCollisionValidation ( )
inline

◆ GetLimbs()

const rbprm::T_Limb& hpp::rbprm::RbPrmFullBody::GetLimbs ( )
inline

◆ GetNonContactingLimbs()

const rbprm::T_Limb& hpp::rbprm::RbPrmFullBody::GetNonContactingLimbs ( )
inline

◆ init()

void hpp::rbprm::RbPrmFullBody::init ( const RbPrmFullBodyWkPtr_t &  weakPtr)
protected

Initialization.

◆ postureWeights() [1/2]

pinocchio::Configuration_t hpp::rbprm::RbPrmFullBody::postureWeights ( )
inline

◆ postureWeights() [2/2]

void hpp::rbprm::RbPrmFullBody::postureWeights ( pinocchio::Configuration_t  postureWeights)

◆ referenceConfig() [1/2]

pinocchio::Configuration_t hpp::rbprm::RbPrmFullBody::referenceConfig ( )
inline

◆ referenceConfig() [2/2]

void hpp::rbprm::RbPrmFullBody::referenceConfig ( pinocchio::Configuration_t  referenceConfig)

◆ setFriction()

void hpp::rbprm::RbPrmFullBody::setFriction ( double  mu)
inline

◆ staticStability() [1/2]

bool hpp::rbprm::RbPrmFullBody::staticStability ( ) const
inline

◆ staticStability() [2/2]

void hpp::rbprm::RbPrmFullBody::staticStability ( bool  staticStability)
inline

◆ toggleNonContactingLimb()

bool hpp::rbprm::RbPrmFullBody::toggleNonContactingLimb ( std::string  name)

◆ usePosturalTaskContactCreation() [1/2]

bool hpp::rbprm::RbPrmFullBody::usePosturalTaskContactCreation ( )
inline

◆ usePosturalTaskContactCreation() [2/2]

void hpp::rbprm::RbPrmFullBody::usePosturalTaskContactCreation ( bool  usePosturalTaskContactCreation)
inline

Member Data Documentation

◆ device_

const pinocchio::DevicePtr_t hpp::rbprm::RbPrmFullBody::device_

The documentation for this class was generated from the following file: