hpp-pinocchio  4.9.1
Wrapping of the kinematic/dynamic chain Pinocchio for HPP.
hpp::pinocchio Namespace Reference

Namespaces

 liegroup
 
 unittest
 
 urdf
 

Classes

class  AbstractDevice
 Abstract class representing a Device. More...
 
class  Body
 
class  CenterOfMassComputation
 
class  CollisionObject
 
struct  DefaultLieGroupMap
 
class  Device
 Robot with geometric and dynamic pinocchio. More...
 
struct  DeviceData
 
class  DeviceSync
 
class  ExtraConfigSpace
 
class  Frame
 Robot frame. More...
 
class  Gripper
 
class  HumanoidRobot
 Humanoid robot. More...
 
class  Joint
 
struct  JointCollectionTpl
 
class  LiegroupElementBase
 
class  LiegroupElementConstBase
 
class  LiegroupSpace
 
class  Pool
 Pool of objects. More...
 
struct  RnxSOnLieGroupMap
 

Typedefs

typedef ::pinocchio::GeometryObject GeometryObject
 
typedef double value_type
 
typedef JointCollectionTpl< value_type, 0 > JointCollection
 
typedef ::pinocchio::JointIndex JointIndex
 
typedef ::pinocchio::FrameIndex FrameIndex
 
typedef ::pinocchio::GeomIndex GeomIndex
 
typedef ::pinocchio::ModelTpl< value_type, 0, JointCollectionTplModel
 
typedef ::pinocchio::DataTpl< value_type, 0, JointCollectionTplData
 
typedef ::pinocchio::GeometryModel GeomModel
 
typedef ::pinocchio::GeometryData GeomData
 
typedef ::pinocchio::SE3 Transform3f
 
typedef ::pinocchio::SE3 SE3
 
typedef ::pinocchio::JointModelTpl< value_type, 0, JointCollectionTplJointModel
 
typedef Eigen::Array< bool, Eigen::Dynamic, 1 > ArrayXb
 
typedef Eigen::Matrix< value_type, Eigen::Dynamic, 1 > vector_t
 
typedef vector_t Configuration_t
 
typedef Eigen::Ref< const Configuration_tConfigurationIn_t
 
typedef Eigen::Ref< Configuration_tConfigurationOut_t
 
typedef boost::shared_ptr< Configuration_tConfigurationPtr_t
 
typedef Eigen::Ref< const vector_tvectorIn_t
 
typedef Eigen::Ref< vector_tvectorOut_t
 
typedef Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > matrix_t
 
typedef Eigen::Ref< matrix_tmatrixOut_t
 
typedef matrix_t::Index size_type
 
typedef Eigen::Matrix< value_type, 3, 3 > matrix3_t
 
typedef Eigen::Matrix< value_type, 3, 1 > vector3_t
 
typedef Eigen::Matrix< value_type, 4, 1 > vector4_t
 
typedef Eigen::Matrix< value_type, 6, Eigen::Dynamic > JointJacobian_t
 
typedef Eigen::Matrix< value_type, 3, Eigen::Dynamic > ComJacobian_t
 
typedef Eigen::Block< JointJacobian_t, 3, Eigen::Dynamic > HalfJointJacobian_t
 
typedef JointVector JointVector_t
 
typedef ObjectVector ObjectVector_t
 
typedef boost::shared_ptr< BodyBodyPtr_t
 
typedef boost::shared_ptr< const BodyBodyConstPtr_t
 
typedef fcl::CollisionObject FclCollisionObject
 
typedef fcl::CollisionObjectFclCollisionObjectPtr_t
 
typedef const fcl::CollisionObjectFclConstCollisionObjectPtr_t
 
typedef boost::shared_ptr< CollisionObjectCollisionObjectPtr_t
 
typedef boost::shared_ptr< const CollisionObjectCollisionObjectConstPtr_t
 
typedef boost::shared_ptr< DeviceDevicePtr_t
 
typedef boost::shared_ptr< const DeviceDeviceConstPtr_t
 
typedef std::vector< fcl::DistanceResultDistanceResults_t
 
typedef boost::shared_ptr< HumanoidRobotHumanoidRobotPtr_t
 
typedef boost::shared_ptr< CenterOfMassComputationCenterOfMassComputationPtr_t
 
typedef boost::shared_ptr< JointJointPtr_t
 
typedef boost::shared_ptr< const JointJointConstPtr_t
 
typedef boost::shared_ptr< GripperGripperPtr_t
 
typedef std::vector< GripperPtr_tGrippers_t
 
typedef boost::shared_ptr< ModelModelPtr_t
 
typedef boost::shared_ptr< const ModelModelConstPtr_t
 
typedef boost::shared_ptr< DataDataPtr_t
 
typedef boost::shared_ptr< const DataDataConstPtr_t
 
typedef boost::shared_ptr< GeomModelGeomModelPtr_t
 
typedef boost::shared_ptr< const GeomModelGeomModelConstPtr_t
 
typedef boost::shared_ptr< GeomDataGeomDataPtr_t
 
typedef boost::shared_ptr< const GeomDataGeomDataConstPtr_t
 
typedef LiegroupElementConstBase< vectorIn_tLiegroupElementConstRef
 Const reference to a LiegroupElement. More...
 
typedef LiegroupElementBase< vector_tLiegroupElement
 Element of a Lie group. More...
 
typedef LiegroupElementBase< vectorOut_tLiegroupElementRef
 Writable reference to a LiegroupElement. More...
 
typedef boost::shared_ptr< LiegroupSpaceLiegroupSpacePtr_t
 
typedef boost::shared_ptr< const LiegroupSpaceLiegroupSpaceConstPtr_t
 
typedef JointCollection::JointModelVariant JointModelVariant
 
typedef JointCollection::JointDataVariant JointDataVariant
 
typedef ABoostVariant LiegroupType
 
typedef ::pinocchio::JointModelCompositeTpl< value_type, 0, JointCollectionTplJointModelComposite
 
typedef HPP_PINOCCHIO_DEPRECATED RnxSOnLieGroupMap LieGroupTpl
 

Enumerations

enum  Computation_t {
  JOINT_POSITION = 0x1, JACOBIAN = 0x2, VELOCITY = 0x4, ACCELERATION = 0x8,
  COM = 0xf, COMPUTE_ALL = 0Xffff
}
 
enum  Request_t { COLLISION, DISTANCE }
 
enum  InOutType { INNER, OUTER }
 
enum  DerivativeProduct { DerivativeTimesInput, InputTimesDerivative }
 

Functions

void saturate (const DevicePtr_t &robot, ConfigurationOut_t configuration)
 
bool saturate (const DevicePtr_t &robot, ConfigurationOut_t configuration, ArrayXb &saturation)
 
template<bool saturateConfig, typename LieGroup >
void integrate (const DevicePtr_t &robot, ConfigurationIn_t configuration, vectorIn_t velocity, ConfigurationOut_t result)
 
template<typename LieGroup >
void interpolate (const DevicePtr_t &robot, ConfigurationIn_t q0, ConfigurationIn_t q1, const value_type &u, ConfigurationOut_t result)
 
template<typename LieGroup >
void difference (const DevicePtr_t &robot, ConfigurationIn_t q1, ConfigurationIn_t q2, vectorOut_t result)
 
bool isApprox (const DevicePtr_t &robot, ConfigurationIn_t q1, ConfigurationIn_t q2, value_type eps)
 
value_type distance (const DevicePtr_t &robot, ConfigurationIn_t q1, ConfigurationIn_t q2)
 
void normalize (const DevicePtr_t &robot, Configuration_t &q)
 
void normalize (const DevicePtr_t &robot, ConfigurationOut_t q)
 
bool isNormalized (const DevicePtr_t &robot, ConfigurationIn_t q, const value_type &eps)
 
std::string displayConfig (ConfigurationIn_t q, int precision=20)
 Write configuration in a string. More...
 
std::ostream & display (std::ostream &os, const SE3 &m)
 Write a SE3 taking into account the indentation. More...
 
std::ostream & operator<< (std::ostream &os, const hpp::pinocchio::Device &device)
 
std::ostream & operator<< (std::ostream &os, const Frame &frame)
 
 HPP_PREDEF_CLASS (Body)
 
 HPP_PREDEF_CLASS (CollisionObject)
 
 HPP_PREDEF_CLASS (Device)
 
 HPP_PREDEF_CLASS (HumanoidRobot)
 
 HPP_PREDEF_CLASS (Joint)
 
 HPP_PREDEF_CLASS (JointConfiguration)
 
 HPP_PREDEF_CLASS (Gripper)
 
 HPP_PREDEF_CLASS (CenterOfMassComputation)
 
 HPP_PREDEF_CLASS (LiegroupSpace)
 
std::ostream & operator<< (std::ostream &os, const Gripper &gripper)
 
std::ostream & operator<< (std::ostream &os, const Joint &joint)
 
template<typename vector_type >
LiegroupElement operator+ (const LiegroupElementConstBase< vector_type > &e, vectorIn_t v)
 
template<typename vector_type1 , typename vector_type2 >
vector_t operator- (const LiegroupElementConstBase< vector_type1 > &e1, const LiegroupElementConstBase< vector_type2 > &e2)
 
template<typename vector_type >
vector_t log (const LiegroupElementConstBase< vector_type > &lge)
 Compute the log as a tangent vector of a Lie group element. More...
 
template<typename vector_type >
std::ostream & operator<< (std::ostream &os, const LiegroupElementConstBase< vector_type > &e)
 
std::ostream & operator<< (std::ostream &os, const LiegroupSpace &space)
 Writing in a stream. More...
 
DevicePtr_t humanoidSimple (const std::string &name="humanoidSimple", bool usingFF=true, Computation_t compFlags=(Computation_t)(JOINT_POSITION|JACOBIAN))
 
DevicePtr_t humanoidSimple (const std::string &name="humanoidSimple", Computation_t compFlags=(Computation_t)(JOINT_POSITION|JACOBIAN))
 

Typedef Documentation

◆ ArrayXb

typedef Eigen::Array<bool, Eigen::Dynamic, 1> hpp::pinocchio::ArrayXb

◆ BodyConstPtr_t

typedef boost::shared_ptr<const Body> hpp::pinocchio::BodyConstPtr_t

◆ BodyPtr_t

typedef boost::shared_ptr<Body> hpp::pinocchio::BodyPtr_t

◆ CenterOfMassComputationPtr_t

◆ CollisionObjectConstPtr_t

◆ CollisionObjectPtr_t

◆ ComJacobian_t

typedef Eigen::Matrix<value_type, 3, Eigen::Dynamic> hpp::pinocchio::ComJacobian_t

◆ Configuration_t

◆ ConfigurationIn_t

◆ ConfigurationOut_t

◆ ConfigurationPtr_t

◆ Data

◆ DataConstPtr_t

typedef boost::shared_ptr<const Data> hpp::pinocchio::DataConstPtr_t

◆ DataPtr_t

typedef boost::shared_ptr<Data> hpp::pinocchio::DataPtr_t

◆ DeviceConstPtr_t

typedef boost::shared_ptr<const Device> hpp::pinocchio::DeviceConstPtr_t

◆ DevicePtr_t

typedef boost::shared_ptr<Device> hpp::pinocchio::DevicePtr_t

◆ DistanceResults_t

◆ FclCollisionObject

◆ FclCollisionObjectPtr_t

◆ FclConstCollisionObjectPtr_t

◆ FrameIndex

◆ GeomData

◆ GeomDataConstPtr_t

typedef boost::shared_ptr<const GeomData> hpp::pinocchio::GeomDataConstPtr_t

◆ GeomDataPtr_t

typedef boost::shared_ptr<GeomData> hpp::pinocchio::GeomDataPtr_t

◆ GeometryObject

◆ GeomIndex

◆ GeomModel

◆ GeomModelConstPtr_t

typedef boost::shared_ptr<const GeomModel> hpp::pinocchio::GeomModelConstPtr_t

◆ GeomModelPtr_t

typedef boost::shared_ptr<GeomModel> hpp::pinocchio::GeomModelPtr_t

◆ GripperPtr_t

typedef boost::shared_ptr<Gripper> hpp::pinocchio::GripperPtr_t

◆ Grippers_t

◆ HalfJointJacobian_t

typedef Eigen::Block<JointJacobian_t, 3, Eigen::Dynamic> hpp::pinocchio::HalfJointJacobian_t

◆ HumanoidRobotPtr_t

◆ JointCollection

◆ JointConstPtr_t

typedef boost::shared_ptr<const Joint> hpp::pinocchio::JointConstPtr_t

◆ JointDataVariant

◆ JointIndex

◆ JointJacobian_t

typedef Eigen::Matrix<value_type, 6, Eigen::Dynamic> hpp::pinocchio::JointJacobian_t

◆ JointModel

◆ JointModelComposite

◆ JointModelVariant

◆ JointPtr_t

typedef boost::shared_ptr<Joint> hpp::pinocchio::JointPtr_t

◆ JointVector_t

typedef JointVector hpp::pinocchio::JointVector_t

◆ LiegroupElement

◆ LiegroupElementConstRef

◆ LiegroupElementRef

◆ LiegroupSpaceConstPtr_t

typedef boost::shared_ptr<const LiegroupSpace> hpp::pinocchio::LiegroupSpaceConstPtr_t

◆ LiegroupSpacePtr_t

◆ LieGroupTpl

◆ matrix3_t

typedef Eigen::Matrix<value_type, 3, 3> hpp::pinocchio::matrix3_t

◆ matrix_t

typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic> hpp::pinocchio::matrix_t

◆ matrixOut_t

◆ Model

◆ ModelConstPtr_t

typedef boost::shared_ptr<const Model> hpp::pinocchio::ModelConstPtr_t

◆ ModelPtr_t

typedef boost::shared_ptr<Model> hpp::pinocchio::ModelPtr_t

◆ ObjectVector_t

typedef ObjectVector hpp::pinocchio::ObjectVector_t

◆ SE3

◆ size_type

typedef matrix_t::Index hpp::pinocchio::size_type

◆ Transform3f

◆ value_type

◆ vector3_t

typedef Eigen::Matrix<value_type, 3, 1> hpp::pinocchio::vector3_t

◆ vector4_t

typedef Eigen::Matrix<value_type, 4, 1> hpp::pinocchio::vector4_t

◆ vector_t

typedef Eigen::Matrix<value_type, Eigen::Dynamic, 1> hpp::pinocchio::vector_t

◆ vectorIn_t

typedef Eigen::Ref<const vector_t> hpp::pinocchio::vectorIn_t

◆ vectorOut_t

Enumeration Type Documentation

◆ Computation_t

Enumerator
JOINT_POSITION 
JACOBIAN 
VELOCITY 
ACCELERATION 
COM 
COMPUTE_ALL 

◆ InOutType

Enumerator
INNER 
OUTER 

◆ Request_t

Enumerator
COLLISION 
DISTANCE 

Function Documentation

◆ difference()

template<typename LieGroup >
void hpp::pinocchio::difference ( const DevicePtr_t robot,
ConfigurationIn_t  q1,
ConfigurationIn_t  q2,
vectorOut_t  result 
)

Difference between two configurations as a vector

Template Parameters
LieGroupis a compile-time map of joint type to Lie group type, that defines the joint-wise operations.
Parameters
robotrobot that describes the kinematic chain
q1first configuration,
q2second configuration,
Return values
resultdifference as a vector $\textbf{v}$ such that q1 is the result of method integrate from q2 with vector $\textbf{v}$
Note
If the configuration space is a vector space, this is $\textbf{v} = q_1 - q_2$
The default template arguments correspond to difference<RnxSOnLieGroupMap>.

◆ display()

std::ostream& hpp::pinocchio::display ( std::ostream &  os,
const SE3 m 
)

Write a SE3 taking into account the indentation.

◆ displayConfig()

std::string hpp::pinocchio::displayConfig ( ConfigurationIn_t  q,
int  precision = 20 
)
inline

Write configuration in a string.

◆ distance()

value_type hpp::pinocchio::distance ( const DevicePtr_t robot,
ConfigurationIn_t  q1,
ConfigurationIn_t  q2 
)

Distance between two configuration.

Parameters
robotrobot that describes the kinematic chain
q1first configuration,
q2second configuration,

◆ HPP_PREDEF_CLASS() [1/9]

hpp::pinocchio::HPP_PREDEF_CLASS ( Body  )

◆ HPP_PREDEF_CLASS() [2/9]

hpp::pinocchio::HPP_PREDEF_CLASS ( CollisionObject  )

◆ HPP_PREDEF_CLASS() [3/9]

hpp::pinocchio::HPP_PREDEF_CLASS ( Device  )

◆ HPP_PREDEF_CLASS() [4/9]

hpp::pinocchio::HPP_PREDEF_CLASS ( HumanoidRobot  )

◆ HPP_PREDEF_CLASS() [5/9]

hpp::pinocchio::HPP_PREDEF_CLASS ( Joint  )

◆ HPP_PREDEF_CLASS() [6/9]

hpp::pinocchio::HPP_PREDEF_CLASS ( JointConfiguration  )

◆ HPP_PREDEF_CLASS() [7/9]

hpp::pinocchio::HPP_PREDEF_CLASS ( Gripper  )

◆ HPP_PREDEF_CLASS() [8/9]

hpp::pinocchio::HPP_PREDEF_CLASS ( CenterOfMassComputation  )

◆ HPP_PREDEF_CLASS() [9/9]

hpp::pinocchio::HPP_PREDEF_CLASS ( LiegroupSpace  )

◆ humanoidSimple() [1/2]

DevicePtr_t hpp::pinocchio::humanoidSimple ( const std::string &  name = "humanoidSimple",
bool  usingFF = true,
Computation_t  compFlags = (Computation_t)(JOINT_POSITION|JACOBIAN) 
)

◆ humanoidSimple() [2/2]

DevicePtr_t hpp::pinocchio::humanoidSimple ( const std::string &  name = "humanoidSimple",
Computation_t  compFlags = (Computation_t)(JOINT_POSITION|JACOBIAN) 
)

◆ integrate()

template<bool saturateConfig, typename LieGroup >
void hpp::pinocchio::integrate ( const DevicePtr_t robot,
ConfigurationIn_t  configuration,
vectorIn_t  velocity,
ConfigurationOut_t  result 
)

Integrate a constant velocity during unit time.

Template Parameters
saturateConfigwhen true, calls hpp::pinocchio::saturate(robot, result) at the end
LieGroupis a compile-time map of joint type to Lie group type, that defines the joint-wise operations.
Parameters
robotrobot that describes the kinematic chain
configurationinitial and result configurations
velocityvelocity vector
Return values
resultconfiguration reached after integration. Velocity is dispatched to each joint that integrates according to its Lie group structure, i.e.
  • $q_i += v_i$ for translation joint and bounded rotation joints,
  • $q_i += v_i \mbox{ modulo } 2\pi$ for unbounded rotation joints,
  • constant rotation velocity for SO(3) joints.
Note
The default template arguments correspond to integrate<true,DefaultLieGroupMap>.

◆ interpolate()

template<typename LieGroup >
void hpp::pinocchio::interpolate ( const DevicePtr_t robot,
ConfigurationIn_t  q0,
ConfigurationIn_t  q1,
const value_type u,
ConfigurationOut_t  result 
)

Interpolate between two configurations of the robot

Template Parameters
LieGroupis a compile-time map of joint type to Lie group type, that defines the joint-wise operations.
Parameters
robotrobot that describes the kinematic chain
q0,q1two configurations to interpolate
uin [0,1] position along the interpolation: q0 for u=0, q1 for u=1
Return values
resultinterpolated configuration
Note
The default template arguments correspond to interpolate<RnxSOnLieGroupMap>.

◆ isApprox()

bool hpp::pinocchio::isApprox ( const DevicePtr_t robot,
ConfigurationIn_t  q1,
ConfigurationIn_t  q2,
value_type  eps 
)

Test that two configurations are close

Parameters
robotrobot that describes the kinematic chain
q1first configuration,
q2second configuration,
epsnumerical threshold
Returns
true if the configurations are closer than the numerical threshold

◆ isNormalized()

bool hpp::pinocchio::isNormalized ( const DevicePtr_t robot,
ConfigurationIn_t  q,
const value_type eps 
)

Check if a configuration is normalized

It consists in checking that norm of quaternions and complex is one.

◆ log()

template<typename vector_type >
vector_t hpp::pinocchio::log ( const LiegroupElementConstBase< vector_type > &  lge)

Compute the log as a tangent vector of a Lie group element.

◆ normalize() [1/2]

void hpp::pinocchio::normalize ( const DevicePtr_t robot,
Configuration_t q 
)

Normalize configuration

Configuration space is a represented by a sub-manifold of a vector space. Normalization consists in projecting a vector on this sub-manifold. It mostly consists in normalizing quaternions for SO3 joints and 2D-vectors for unbounded rotations.

◆ normalize() [2/2]

void hpp::pinocchio::normalize ( const DevicePtr_t robot,
ConfigurationOut_t  q 
)
inline

For backward compatibility. See normalize normalize (const DevicePtr_t&, Configuration_t&)

◆ operator<<() [1/5]

std::ostream& hpp::pinocchio::operator<< ( std::ostream &  os,
const Gripper gripper 
)
inline

◆ operator<<() [2/5]

std::ostream& hpp::pinocchio::operator<< ( std::ostream &  os,
const Frame frame 
)
inline

◆ operator<<() [3/5]

template<typename vector_type >
std::ostream& hpp::pinocchio::operator<< ( std::ostream &  os,
const LiegroupElementConstBase< vector_type > &  e 
)
inline

◆ operator<<() [4/5]

std::ostream& hpp::pinocchio::operator<< ( std::ostream &  os,
const Joint joint 
)
inline

◆ operator<<() [5/5]

std::ostream& hpp::pinocchio::operator<< ( std::ostream &  os,
const hpp::pinocchio::Device device 
)
inline

◆ saturate() [1/2]

void hpp::pinocchio::saturate ( const DevicePtr_t robot,
ConfigurationOut_t  configuration 
)

Saturate joint parameter value so that they are not out of bounds.

Parameters
robotrobot that describes the kinematic chain
[in,out]configurationinitial and result configurations
Return values
configurationreached after saturation.

◆ saturate() [2/2]

bool hpp::pinocchio::saturate ( const DevicePtr_t robot,
ConfigurationOut_t  configuration,
ArrayXb saturation 
)

Saturate joint parameter value so that they are not out of bounds.

Parameters
robotrobot that describes the kinematic chain
[in,out]configurationinitial and result configurations
Return values
saturationan array of boolean saying who saturated (size robot.numberDof()).