Public Types | Public Member Functions | Protected Member Functions | Protected Attributes | Friends | List of all members
hpp::pinocchio::Device Class Reference

Robot with geometric and dynamic pinocchio. More...

#include <hpp/pinocchio/device.hh>

Inheritance diagram for hpp::pinocchio::Device:
[legend]
Collaboration diagram for hpp::pinocchio::Device:
[legend]

Public Types

enum  Computation_t {
  JOINT_POSITION = 0x1,
  JACOBIAN = 0x2,
  VELOCITY = 0x4,
  ACCELERATION = 0x8,
  COM = 0x10,
  ALL = 0Xffff
}
 Flags to select computation To optimize computation time, computations performed by method computeForwardKinematics can be selected by calling method controlComputation. More...
 
typedef std::pair< JointPtr_t, JointPtr_tCollisionPair_t
 Collision pairs between bodies. More...
 
typedef std::list< CollisionPair_tCollisionPairs_t
 

Public Member Functions

virtual std::ostream & print (std::ostream &os) const
 Print object in a stream. More...
 
fcl::AABB computeAABB () const
 Compute an aligned bounding around the robot. More...
 
Access to pinocchio API
void model (ModelPtr_t modelPtr)
 Set pinocchio model. More...
 
ModelConstPtr_t modelPtr () const
 Access to pinocchio model. More...
 
ModelPtr_t modelPtr ()
 Access to pinocchio model. More...
 
const Modelmodel () const
 Access to pinocchio model. More...
 
Modelmodel ()
 Access to pinocchio model. More...
 
void geomModel (GeomModelPtr_t geomModelPtr)
 Set pinocchio geom. More...
 
GeomModelConstPtr_t geomModelPtr () const
 Access to pinocchio geomModel. More...
 
GeomModelPtr_t geomModelPtr ()
 Access to pinocchio geomModel. More...
 
const GeomModelgeomModel () const
 Access to pinocchio geomModel. More...
 
GeomModelgeomModel ()
 Access to pinocchio geomModel. More...
 
void data (DataPtr_t dataPtr)
 Set Pinocchio data corresponding to model. More...
 
DataConstPtr_t dataPtr () const
 Access to Pinocchio data/. More...
 
DataPtr_t dataPtr ()
 Access to Pinocchio data/. More...
 
const Datadata () const
 Access to Pinocchio data/. More...
 
Datadata ()
 Access to Pinocchio data/. More...
 
void createData ()
 Create Pinocchio data from model. More...
 
void geomData (GeomDataPtr_t geomDataPtr)
 Set Pinocchio geomData corresponding to model. More...
 
GeomDataConstPtr_t geomDataPtr () const
 Access to Pinocchio geomData/. More...
 
GeomDataPtr_t geomDataPtr ()
 Access to Pinocchio geomData/. More...
 
const GeomDatageomData () const
 Access to Pinocchio geomData/. More...
 
GeomDatageomData ()
 Access to Pinocchio geomData/. More...
 
void createGeomData ()
 Create Pinocchio geomData from model. More...
 
Joints
JointPtr_t rootJoint () const
 Get root joint. More...
 
Frame rootFrame () const
 Get root frame. More...
 
const JointVectorgetJointVector () const
 Get vector of joints. More...
 
JointVectorgetJointVector ()
 
JointPtr_t getJointAtConfigRank (const size_type &r) const
 Get the joint at configuration rank r. More...
 
JointPtr_t getJointAtVelocityRank (const size_type &r) const
 Get the joint at velocity rank r. More...
 
JointPtr_t getJointByName (const std::string &name) const
 Get joint by name. More...
 
JointPtr_t getJointByBodyName (const std::string &name) const
 Get joint by body name. More...
 
Frame getFrameByName (const std::string &name) const
 Get frame by name. More...
 
size_type configSize () const
 Size of configuration vectors Sum of joint dimensions and of extra configuration space dimension. More...
 
size_type numberDof () const
 Size of velocity vectors Sum of joint number of degrees of freedom and of extra configuration space dimension. More...
 
const LiegroupSpacePtr_tconfigSpace () const
 Returns a LiegroupSpace representing the configuration space. More...
 
Extra configuration space
ExtraConfigSpaceextraConfigSpace ()
 Get degrees of freedom to store internal values in configurations. More...
 
const ExtraConfigSpaceextraConfigSpace () const
 Get degrees of freedom to store internal values in configurations. More...
 
virtual void setDimensionExtraConfigSpace (const size_type &dimension)
 Set dimension of extra configuration space. More...
 
Current state
const Configuration_tcurrentConfiguration () const
 Get current configuration. More...
 
virtual bool currentConfiguration (ConfigurationIn_t configuration)
 Set current configuration. More...
 
Configuration_t neutralConfiguration () const
 Get the neutral configuration. More...
 
const vector_tcurrentVelocity () const
 Get current velocity. More...
 
void currentVelocity (vectorIn_t velocity)
 Set current velocity. More...
 
const vector_tcurrentAcceleration () const
 Get current acceleration. More...
 
void currentAcceleration (vectorIn_t acceleration)
 Set current acceleration. More...
 
Mass and center of mass
const value_typemass () const
 Get mass of robot. More...
 
const vector3_tpositionCenterOfMass () const
 Get position of center of mass. More...
 
const ComJacobian_tjacobianCenterOfMass () const
 Get Jacobian of center of mass with respect to configuration. More...
 
Grippers
void addGripper (const GripperPtr_t &gripper)
 Add a gripper to the Device. More...
 
Grippers_tgrippers ()
 Return list of grippers of the Device. More...
 
const Grippers_tgrippers () const
 Return list of grippers of the Device. More...
 
Collision and distance computation
const ObjectVector_tobstacles () const
 Get list of obstacles. More...
 
DeviceObjectVectorobjectVector ()
 Vector of inner objects of the device. More...
 
const DeviceObjectVectorobjectVector () const
 
bool collisionTest (const bool stopAtFirstCollision=true)
 Test collision of current configuration. More...
 
void computeDistances ()
 Compute distances between pairs of objects stored in bodies. More...
 
const DistanceResults_tdistanceResults () const
 Get result of distance computations. More...
 
Forward kinematics
void controlComputation (const Computation_t &flag)
 Select computation Optimize computation time by selecting only necessary values in method computeForwardKinematics. More...
 
Computation_t computationFlag () const
 Get computation flag. More...
 
void computeForwardKinematics ()
 Compute forward kinematics. More...
 
void computeFramesForwardKinematics ()
 Compute frame forward kinematics. More...
 
void updateGeometryPlacements ()
 Update the geometry placement to the currentConfiguration. More...
 

Protected Member Functions

 Device (const std::string &name)
 Constructor. More...
 
void init (const DeviceWkPtr_t &weakPtr)
 Initialization. More...
 
void initCopy (const DeviceWkPtr_t &weakPtr, const Device &other)
 Initialization of of a clone device. More...
 
 Device (const Device &device)
 Copy Constructor. More...
 
void invalidate ()
 

Protected Attributes

ModelPtr_t model_
 
DataPtr_t data_
 
GeomModelPtr_t geomModel_
 
GeomDataPtr_t geomData_
 
std::string name_
 
JointVector jointVector_
 
Configuration_t currentConfiguration_
 
vector_t currentVelocity_
 
vector_t currentAcceleration_
 
bool upToDate_
 
bool frameUpToDate_
 
bool geomUpToDate_
 
Computation_t computationFlag_
 
ObjectVector_t obstacles_
 
DeviceObjectVector objectVector_
 
Grippers_t grippers_
 
LiegroupSpacePtr_t configSpace_
 
ExtraConfigSpace extraConfigSpace_
 
DeviceWkPtr_t weakPtr_
 
Configuration_t robotConf_
 Temporary variable to avoid dynamic allocation. More...
 

Friends

class Joint
 
class Frame
 

Construction, copy and destruction

virtual ~Device ()
 
virtual DevicePtr_t clone ()
 Clone as a CkwsDevice The pinocchio model is not copied (only copy the pointer). More...
 
DevicePtr_t cloneConst () const
 Clone as a CkwsDevice Both pinocchio objects model and data are copied. More...
 
const std::string & name () const
 Get name of device. More...
 
static DevicePtr_t create (const std::string &name)
 Creation of a new device. More...
 
static DevicePtr_t createCopy (const DevicePtr_t &device)
 Copy of a device. More...
 
static DevicePtr_t createCopyConst (const DeviceConstPtr_t &device)
 

Detailed Description

Robot with geometric and dynamic pinocchio.

The creation of the device is done by Device::create(const std::string name). This function returns a shared pointer to the newly created object.

See also
Smart pointers documentation: http://www.boost.org/libs/smart_ptr/smart_ptr.htm

Member Typedef Documentation

◆ CollisionPair_t

Collision pairs between bodies.

◆ CollisionPairs_t

Member Enumeration Documentation

◆ Computation_t

Flags to select computation To optimize computation time, computations performed by method computeForwardKinematics can be selected by calling method controlComputation.

Enumerator
JOINT_POSITION 
JACOBIAN 
VELOCITY 
ACCELERATION 
COM 
ALL 

Constructor & Destructor Documentation

◆ ~Device()

virtual hpp::pinocchio::Device::~Device ( )
inlinevirtual

◆ Device() [1/2]

hpp::pinocchio::Device::Device ( const std::string &  name)
protected

Constructor.

◆ Device() [2/2]

hpp::pinocchio::Device::Device ( const Device device)
protected

Copy Constructor.

Member Function Documentation

◆ addGripper()

void hpp::pinocchio::Device::addGripper ( const GripperPtr_t gripper)
inline

Add a gripper to the Device.

◆ clone()

virtual DevicePtr_t hpp::pinocchio::Device::clone ( )
inlinevirtual

Clone as a CkwsDevice The pinocchio model is not copied (only copy the pointer).

A new Pinocchio "data" is created. As the model is not copied, cloning is a non constant operation.

See also
cloneConst

◆ cloneConst()

DevicePtr_t hpp::pinocchio::Device::cloneConst ( ) const
inline

Clone as a CkwsDevice Both pinocchio objects model and data are copied.

TODO: this method is not implemented yet (assert if called)

◆ collisionTest()

bool hpp::pinocchio::Device::collisionTest ( const bool  stopAtFirstCollision = true)

Test collision of current configuration.

Parameters
stopAtFirstCollisionact as named
Warning
Users should call computeForwardKinematics first.

◆ computationFlag()

Computation_t hpp::pinocchio::Device::computationFlag ( ) const
inline

Get computation flag.

◆ computeAABB()

fcl::AABB hpp::pinocchio::Device::computeAABB ( ) const

Compute an aligned bounding around the robot.

The bounding box is computed as follows:

  • for each direct children of the universe joint:
    • compute a bounding box of its center (using the bounds in translation)
    • compute the maximal distance between the its center and each bodies attach its subtree.
    • sum the two.
  • sum all the BB obtained.

◆ computeDistances()

void hpp::pinocchio::Device::computeDistances ( )

Compute distances between pairs of objects stored in bodies.

Warning
Users should call computeForwardKinematics first.

◆ computeForwardKinematics()

void hpp::pinocchio::Device::computeForwardKinematics ( )

Compute forward kinematics.

◆ computeFramesForwardKinematics()

void hpp::pinocchio::Device::computeFramesForwardKinematics ( )

Compute frame forward kinematics.

◆ configSize()

size_type hpp::pinocchio::Device::configSize ( ) const

Size of configuration vectors Sum of joint dimensions and of extra configuration space dimension.

◆ configSpace()

const LiegroupSpacePtr_t& hpp::pinocchio::Device::configSpace ( ) const
inline

Returns a LiegroupSpace representing the configuration space.

◆ controlComputation()

void hpp::pinocchio::Device::controlComputation ( const Computation_t flag)
inline

Select computation Optimize computation time by selecting only necessary values in method computeForwardKinematics.

◆ create()

static DevicePtr_t hpp::pinocchio::Device::create ( const std::string &  name)
static

Creation of a new device.

Returns
a shared pointer to the new device
Parameters
nameName of the device

◆ createCopy()

static DevicePtr_t hpp::pinocchio::Device::createCopy ( const DevicePtr_t device)
static

Copy of a device.

Returns
A shared pointer to new device.
Parameters
deviceDevice to be copied. The pinocchio model is not copied (only copy the pointer). A new Pinocchio "data" is created.

◆ createCopyConst()

static DevicePtr_t hpp::pinocchio::Device::createCopyConst ( const DeviceConstPtr_t device)
static

◆ createData()

void hpp::pinocchio::Device::createData ( )

Create Pinocchio data from model.

◆ createGeomData()

void hpp::pinocchio::Device::createGeomData ( )

Create Pinocchio geomData from model.

◆ currentAcceleration() [1/2]

const vector_t& hpp::pinocchio::Device::currentAcceleration ( ) const
inline

Get current acceleration.

◆ currentAcceleration() [2/2]

void hpp::pinocchio::Device::currentAcceleration ( vectorIn_t  acceleration)
inline

Set current acceleration.

◆ currentConfiguration() [1/2]

const Configuration_t& hpp::pinocchio::Device::currentConfiguration ( ) const
inline

Get current configuration.

◆ currentConfiguration() [2/2]

virtual bool hpp::pinocchio::Device::currentConfiguration ( ConfigurationIn_t  configuration)
virtual

Set current configuration.

Returns
True if the current configuration was modified and false if the current configuration did not change.

◆ currentVelocity() [1/2]

const vector_t& hpp::pinocchio::Device::currentVelocity ( ) const
inline

Get current velocity.

◆ currentVelocity() [2/2]

void hpp::pinocchio::Device::currentVelocity ( vectorIn_t  velocity)
inline

Set current velocity.

◆ data() [1/3]

void hpp::pinocchio::Device::data ( DataPtr_t  dataPtr)
inline

Set Pinocchio data corresponding to model.

◆ data() [2/3]

const Data& hpp::pinocchio::Device::data ( ) const
inline

Access to Pinocchio data/.

◆ data() [3/3]

Data& hpp::pinocchio::Device::data ( )
inline

Access to Pinocchio data/.

◆ dataPtr() [1/2]

DataConstPtr_t hpp::pinocchio::Device::dataPtr ( ) const
inline

Access to Pinocchio data/.

◆ dataPtr() [2/2]

DataPtr_t hpp::pinocchio::Device::dataPtr ( )
inline

Access to Pinocchio data/.

◆ distanceResults()

const DistanceResults_t& hpp::pinocchio::Device::distanceResults ( ) const

Get result of distance computations.

◆ extraConfigSpace() [1/2]

ExtraConfigSpace& hpp::pinocchio::Device::extraConfigSpace ( )
inline

Get degrees of freedom to store internal values in configurations.

In some applications, it is useful to store extra variables with the configuration vector. For instance, when planning motions in state space using roadmap based methods, the velocity of the robot is stored in the nodes of the roadmap.

◆ extraConfigSpace() [2/2]

const ExtraConfigSpace& hpp::pinocchio::Device::extraConfigSpace ( ) const
inline

Get degrees of freedom to store internal values in configurations.

In some applications, it is useful to store extra variables with the configuration vector. For instance, when planning motions in state space using roadmap based methods, the velocity of the robot is stored in the nodes of the roadmap.

◆ geomData() [1/3]

void hpp::pinocchio::Device::geomData ( GeomDataPtr_t  geomDataPtr)
inline

Set Pinocchio geomData corresponding to model.

◆ geomData() [2/3]

const GeomData& hpp::pinocchio::Device::geomData ( ) const
inline

Access to Pinocchio geomData/.

◆ geomData() [3/3]

GeomData& hpp::pinocchio::Device::geomData ( )
inline

Access to Pinocchio geomData/.

◆ geomDataPtr() [1/2]

GeomDataConstPtr_t hpp::pinocchio::Device::geomDataPtr ( ) const
inline

Access to Pinocchio geomData/.

◆ geomDataPtr() [2/2]

GeomDataPtr_t hpp::pinocchio::Device::geomDataPtr ( )
inline

Access to Pinocchio geomData/.

◆ geomModel() [1/3]

void hpp::pinocchio::Device::geomModel ( GeomModelPtr_t  geomModelPtr)
inline

Set pinocchio geom.

◆ geomModel() [2/3]

const GeomModel& hpp::pinocchio::Device::geomModel ( ) const
inline

Access to pinocchio geomModel.

◆ geomModel() [3/3]

GeomModel& hpp::pinocchio::Device::geomModel ( )
inline

Access to pinocchio geomModel.

◆ geomModelPtr() [1/2]

GeomModelConstPtr_t hpp::pinocchio::Device::geomModelPtr ( ) const
inline

Access to pinocchio geomModel.

◆ geomModelPtr() [2/2]

GeomModelPtr_t hpp::pinocchio::Device::geomModelPtr ( )
inline

Access to pinocchio geomModel.

◆ getFrameByName()

Frame hpp::pinocchio::Device::getFrameByName ( const std::string &  name) const

Get frame by name.

Parameters
namename of the frame.
Exceptions
runtime_errorif device has no frame with this name

◆ getJointAtConfigRank()

JointPtr_t hpp::pinocchio::Device::getJointAtConfigRank ( const size_type r) const

Get the joint at configuration rank r.

Returns
joint j such that j->rankInConfiguration () <= r < j->rankInConfiguration () + j->configSize ()

◆ getJointAtVelocityRank()

JointPtr_t hpp::pinocchio::Device::getJointAtVelocityRank ( const size_type r) const

Get the joint at velocity rank r.

Returns
joint j such that j->rankInVelocity () <= r < j->rankInVelocity () + j->numberDof ()

◆ getJointByBodyName()

JointPtr_t hpp::pinocchio::Device::getJointByBodyName ( const std::string &  name) const

Get joint by body name.

Exceptions
runtime_errorif device has no body with this name

◆ getJointByName()

JointPtr_t hpp::pinocchio::Device::getJointByName ( const std::string &  name) const

Get joint by name.

Parameters
namename of the joint.
Exceptions
runtime_errorif device has no joint with this name

◆ getJointVector() [1/2]

const JointVector& hpp::pinocchio::Device::getJointVector ( ) const
inline

Get vector of joints.

◆ getJointVector() [2/2]

JointVector& hpp::pinocchio::Device::getJointVector ( )
inline

◆ grippers() [1/2]

Grippers_t& hpp::pinocchio::Device::grippers ( )
inline

Return list of grippers of the Device.

◆ grippers() [2/2]

const Grippers_t& hpp::pinocchio::Device::grippers ( ) const
inline

Return list of grippers of the Device.

◆ init()

void hpp::pinocchio::Device::init ( const DeviceWkPtr_t &  weakPtr)
protected

Initialization.

◆ initCopy()

void hpp::pinocchio::Device::initCopy ( const DeviceWkPtr_t &  weakPtr,
const Device other 
)
protected

Initialization of of a clone device.

◆ invalidate()

void hpp::pinocchio::Device::invalidate ( )
inlineprotected

◆ jacobianCenterOfMass()

const ComJacobian_t& hpp::pinocchio::Device::jacobianCenterOfMass ( ) const

Get Jacobian of center of mass with respect to configuration.

◆ mass()

const value_type& hpp::pinocchio::Device::mass ( ) const

Get mass of robot.

◆ model() [1/3]

void hpp::pinocchio::Device::model ( ModelPtr_t  modelPtr)
inline

Set pinocchio model.

◆ model() [2/3]

const Model& hpp::pinocchio::Device::model ( ) const
inline

Access to pinocchio model.

◆ model() [3/3]

Model& hpp::pinocchio::Device::model ( )
inline

Access to pinocchio model.

◆ modelPtr() [1/2]

ModelConstPtr_t hpp::pinocchio::Device::modelPtr ( ) const
inline

Access to pinocchio model.

◆ modelPtr() [2/2]

ModelPtr_t hpp::pinocchio::Device::modelPtr ( )
inline

Access to pinocchio model.

◆ name()

const std::string& hpp::pinocchio::Device::name ( ) const
inline

Get name of device.

◆ neutralConfiguration()

Configuration_t hpp::pinocchio::Device::neutralConfiguration ( ) const

Get the neutral configuration.

◆ numberDof()

size_type hpp::pinocchio::Device::numberDof ( ) const

Size of velocity vectors Sum of joint number of degrees of freedom and of extra configuration space dimension.

◆ objectVector() [1/2]

DeviceObjectVector& hpp::pinocchio::Device::objectVector ( )
inline

Vector of inner objects of the device.

◆ objectVector() [2/2]

const DeviceObjectVector& hpp::pinocchio::Device::objectVector ( ) const
inline

◆ obstacles()

const ObjectVector_t& hpp::pinocchio::Device::obstacles ( ) const
inline

Get list of obstacles.

◆ positionCenterOfMass()

const vector3_t& hpp::pinocchio::Device::positionCenterOfMass ( ) const

Get position of center of mass.

◆ print()

virtual std::ostream& hpp::pinocchio::Device::print ( std::ostream &  os) const
virtual

Print object in a stream.

Referenced by hpp::pinocchio::operator<<().

◆ rootFrame()

Frame hpp::pinocchio::Device::rootFrame ( ) const

Get root frame.

◆ rootJoint()

JointPtr_t hpp::pinocchio::Device::rootJoint ( ) const

Get root joint.

◆ setDimensionExtraConfigSpace()

virtual void hpp::pinocchio::Device::setDimensionExtraConfigSpace ( const size_type dimension)
inlinevirtual

Set dimension of extra configuration space.

◆ updateGeometryPlacements()

void hpp::pinocchio::Device::updateGeometryPlacements ( )

Update the geometry placement to the currentConfiguration.

Friends And Related Function Documentation

◆ Frame

friend class Frame
friend

◆ Joint

friend class Joint
friend

Member Data Documentation

◆ computationFlag_

Computation_t hpp::pinocchio::Device::computationFlag_
protected

◆ configSpace_

LiegroupSpacePtr_t hpp::pinocchio::Device::configSpace_
protected

◆ currentAcceleration_

vector_t hpp::pinocchio::Device::currentAcceleration_
protected

◆ currentConfiguration_

Configuration_t hpp::pinocchio::Device::currentConfiguration_
protected

◆ currentVelocity_

vector_t hpp::pinocchio::Device::currentVelocity_
protected

◆ data_

DataPtr_t hpp::pinocchio::Device::data_
protected

◆ extraConfigSpace_

ExtraConfigSpace hpp::pinocchio::Device::extraConfigSpace_
protected

◆ frameUpToDate_

bool hpp::pinocchio::Device::frameUpToDate_
protected

◆ geomData_

GeomDataPtr_t hpp::pinocchio::Device::geomData_
protected

◆ geomModel_

GeomModelPtr_t hpp::pinocchio::Device::geomModel_
protected

◆ geomUpToDate_

bool hpp::pinocchio::Device::geomUpToDate_
protected

◆ grippers_

Grippers_t hpp::pinocchio::Device::grippers_
protected

◆ jointVector_

JointVector hpp::pinocchio::Device::jointVector_
protected

◆ model_

ModelPtr_t hpp::pinocchio::Device::model_
protected

◆ name_

std::string hpp::pinocchio::Device::name_
protected

◆ objectVector_

DeviceObjectVector hpp::pinocchio::Device::objectVector_
protected

◆ obstacles_

ObjectVector_t hpp::pinocchio::Device::obstacles_
protected

◆ robotConf_

Configuration_t hpp::pinocchio::Device::robotConf_
mutableprotected

Temporary variable to avoid dynamic allocation.

◆ upToDate_

bool hpp::pinocchio::Device::upToDate_
protected

◆ weakPtr_

DeviceWkPtr_t hpp::pinocchio::Device::weakPtr_
protected