Robot with geometric and dynamic pinocchio. More...
#include <hpp/pinocchio/device.hh>
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_t > | CollisionPair_t |
Collision pairs between bodies. More... | |
typedef std::list< CollisionPair_t > | CollisionPairs_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 Model & | model () const |
Access to pinocchio model. More... | |
Model & | model () |
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 GeomModel & | geomModel () const |
Access to pinocchio geomModel. More... | |
GeomModel & | geomModel () |
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 Data & | data () const |
Access to Pinocchio data/. More... | |
Data & | data () |
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 GeomData & | geomData () const |
Access to Pinocchio geomData/. More... | |
GeomData & | geomData () |
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 JointVector & | getJointVector () const |
Get vector of joints. More... | |
JointVector & | getJointVector () |
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_t & | configSpace () const |
Returns a LiegroupSpace representing the configuration space. More... | |
Extra configuration space | |
ExtraConfigSpace & | extraConfigSpace () |
Get degrees of freedom to store internal values in configurations. More... | |
const ExtraConfigSpace & | extraConfigSpace () 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_t & | currentConfiguration () 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_t & | currentVelocity () const |
Get current velocity. More... | |
void | currentVelocity (vectorIn_t velocity) |
Set current velocity. More... | |
const vector_t & | currentAcceleration () const |
Get current acceleration. More... | |
void | currentAcceleration (vectorIn_t acceleration) |
Set current acceleration. More... | |
Mass and center of mass | |
const value_type & | mass () const |
Get mass of robot. More... | |
const vector3_t & | positionCenterOfMass () const |
Get position of center of mass. More... | |
const ComJacobian_t & | jacobianCenterOfMass () 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_t & | grippers () |
Return list of grippers of the Device. More... | |
const Grippers_t & | grippers () const |
Return list of grippers of the Device. More... | |
Collision and distance computation | |
const ObjectVector_t & | obstacles () const |
Get list of obstacles. More... | |
DeviceObjectVector & | objectVector () |
Vector of inner objects of the device. More... | |
const DeviceObjectVector & | objectVector () 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_t & | distanceResults () 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 () const |
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) |
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.
typedef std::pair<JointPtr_t, JointPtr_t> hpp::pinocchio::Device::CollisionPair_t |
Collision pairs between bodies.
typedef std::list<CollisionPair_t> hpp::pinocchio::Device::CollisionPairs_t |
|
inlinevirtual |
|
protected |
Constructor.
|
protected |
Copy Constructor.
|
inline |
Add a gripper to the Device.
|
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.
Reimplemented in hpp::pinocchio::HumanoidRobot.
|
inline |
Clone as a CkwsDevice Both pinocchio objects model and data are copied.
TODO: this method is not implemented yet (assert if called)
bool hpp::pinocchio::Device::collisionTest | ( | const bool | stopAtFirstCollision = true | ) |
Test collision of current configuration.
stopAtFirstCollision | act as named |
|
inline |
Get computation flag.
fcl::AABB hpp::pinocchio::Device::computeAABB | ( | ) | const |
Compute an aligned bounding around the robot.
The bounding box is computed as follows:
void hpp::pinocchio::Device::computeDistances | ( | ) |
Compute distances between pairs of objects stored in bodies.
void hpp::pinocchio::Device::computeForwardKinematics | ( | ) |
Compute forward kinematics.
void hpp::pinocchio::Device::computeFramesForwardKinematics | ( | ) |
Compute frame forward kinematics.
size_type hpp::pinocchio::Device::configSize | ( | ) | const |
Size of configuration vectors Sum of joint dimensions and of extra configuration space dimension.
|
inline |
Returns a LiegroupSpace representing the configuration space.
|
inline |
Select computation Optimize computation time by selecting only necessary values in method computeForwardKinematics.
|
static |
Creation of a new device.
name | Name of the device |
|
static |
Copy of a device.
device | Device to be copied. The pinocchio model is not copied (only copy the pointer). A new Pinocchio "data" is created. |
|
static |
void hpp::pinocchio::Device::createData | ( | ) |
Create Pinocchio data from model.
void hpp::pinocchio::Device::createGeomData | ( | ) |
Create Pinocchio geomData from model.
|
inline |
Get current acceleration.
|
inline |
Set current acceleration.
|
inline |
Get current configuration.
|
virtual |
Set current configuration.
|
inline |
Get current velocity.
|
inline |
Set current velocity.
|
inline |
Set Pinocchio data corresponding to model.
|
inline |
Access to Pinocchio data/.
|
inline |
Access to Pinocchio data/.
|
inline |
Access to Pinocchio data/.
|
inline |
Access to Pinocchio data/.
const DistanceResults_t& hpp::pinocchio::Device::distanceResults | ( | ) | const |
Get result of distance computations.
|
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.
|
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.
|
inline |
Set Pinocchio geomData corresponding to model.
|
inline |
Access to Pinocchio geomData/.
|
inline |
Access to Pinocchio geomData/.
|
inline |
Access to Pinocchio geomData/.
|
inline |
Access to Pinocchio geomData/.
|
inline |
Set pinocchio geom.
|
inline |
Access to pinocchio geomModel.
|
inline |
Access to pinocchio geomModel.
|
inline |
Access to pinocchio geomModel.
|
inline |
Access to pinocchio geomModel.
Frame hpp::pinocchio::Device::getFrameByName | ( | const std::string & | name | ) | const |
Get frame by name.
name | name of the frame. |
runtime_error | if device has no frame with this name |
JointPtr_t hpp::pinocchio::Device::getJointAtConfigRank | ( | const size_type & | r | ) | const |
Get the joint at configuration rank r.
JointPtr_t hpp::pinocchio::Device::getJointAtVelocityRank | ( | const size_type & | r | ) | const |
Get the joint at velocity rank r.
JointPtr_t hpp::pinocchio::Device::getJointByBodyName | ( | const std::string & | name | ) | const |
Get joint by body name.
runtime_error | if device has no body with this name |
JointPtr_t hpp::pinocchio::Device::getJointByName | ( | const std::string & | name | ) | const |
Get joint by name.
name | name of the joint. |
runtime_error | if device has no joint with this name |
|
inline |
Get vector of joints.
|
inline |
|
inline |
Return list of grippers of the Device.
|
inline |
Return list of grippers of the Device.
|
protected |
Initialization.
|
protected |
Initialization of of a clone device.
|
inlineprotected |
const ComJacobian_t& hpp::pinocchio::Device::jacobianCenterOfMass | ( | ) | const |
Get Jacobian of center of mass with respect to configuration.
const value_type& hpp::pinocchio::Device::mass | ( | ) | const |
Get mass of robot.
|
inline |
Set pinocchio model.
|
inline |
Access to pinocchio model.
|
inline |
Access to pinocchio model.
|
inline |
Access to pinocchio model.
|
inline |
Access to pinocchio model.
|
inline |
Get name of device.
Configuration_t hpp::pinocchio::Device::neutralConfiguration | ( | ) | const |
Get the neutral configuration.
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.
|
inline |
Vector of inner objects of the device.
|
inline |
|
inline |
Get list of obstacles.
const vector3_t& hpp::pinocchio::Device::positionCenterOfMass | ( | ) | const |
Get position of center of mass.
|
virtual |
Print object in a stream.
Referenced by hpp::pinocchio::operator<<().
Frame hpp::pinocchio::Device::rootFrame | ( | ) | const |
Get root frame.
JointPtr_t hpp::pinocchio::Device::rootJoint | ( | ) | const |
Get root joint.
|
inlinevirtual |
Set dimension of extra configuration space.
void hpp::pinocchio::Device::updateGeometryPlacements | ( | ) |
Update the geometry placement to the currentConfiguration.
|
friend |
|
friend |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
mutableprotected |
Temporary variable to avoid dynamic allocation.
|
protected |
|
protected |