se3 Namespace Reference

Namespaces

namespace  buildModels
namespace  cholesky
namespace  forceSet
namespace  fusion
namespace  internal
namespace  lua
namespace  motionSet
namespace  prismatic
namespace  python
namespace  revolute
namespace  srdf
namespace  urdf

Classes

struct  CATForwardStep
struct  CATBackwardStep
struct  IntegrateStep
struct  InterpolateStep
struct  DifferentiateStep
struct  DistanceStep
struct  RandomConfiguration
struct  emptyForwardStep
struct  ForwardKinematicZeroStep
struct  ForwardKinematicFirstStep
struct  ForwardKinematicSecondStep
class  Exception
class  ConstraintBase
struct  traits< ConstraintTpl< D, T, U > >
class  ConstraintTpl
class  ForceSetTpl
struct  CollisionPair
struct  DistanceResult
 Result of distance computation between two CollisionObjects. More...
struct  CollisionResult
 Result of collision computation between two CollisionObjects. More...
struct  GeometryObject
struct  GeometryModel
struct  GeometryData
struct  traits< JointAccessor >
struct  traits< JointDataAccessor >
struct  traits< JointModelAccessor >
struct  JointDataAccessor
struct  JointModelAccessor
struct  JointDataBase
struct  SizeDepType
struct  SizeDepType< Eigen::Dynamic >
struct  JointModelBase
struct  traits< JointDense< _NQ, _NV > >
struct  traits< JointDataDense< _NQ, _NV > >
struct  traits< JointModelDense< _NQ, _NV > >
struct  JointDataDense
struct  JointModelDense
struct  traits< ConstraintIdentity >
struct  ConstraintIdentity
struct  traits< JointFreeFlyer >
struct  traits< JointDataFreeFlyer >
struct  traits< JointModelFreeFlyer >
struct  JointDataFreeFlyer
struct  JointModelFreeFlyer
struct  traits< JointGeneric >
struct  traits< JointDataGeneric >
struct  traits< JointModelGeneric >
struct  JointDataGeneric
struct  JointModelGeneric
struct  traits< MotionPlanar >
struct  MotionPlanar
struct  traits< ConstraintPlanar >
struct  ConstraintPlanar
struct  traits< JointPlanar >
struct  traits< JointDataPlanar >
struct  traits< JointModelPlanar >
struct  JointDataPlanar
struct  JointModelPlanar
struct  traits< MotionPrismaticUnaligned >
struct  MotionPrismaticUnaligned
struct  traits< ConstraintPrismaticUnaligned >
struct  ConstraintPrismaticUnaligned
struct  traits< JointPrismaticUnaligned >
struct  traits< JointDataPrismaticUnaligned >
struct  traits< JointModelPrismaticUnaligned >
struct  JointDataPrismaticUnaligned
struct  JointModelPrismaticUnaligned
struct  traits< MotionPrismatic< axis > >
struct  MotionPrismatic
struct  traits< ConstraintPrismatic< axis > >
struct  ConstraintPrismatic
struct  JointPrismatic
struct  traits< JointPrismatic< axis > >
struct  traits< JointDataPrismatic< axis > >
struct  traits< JointModelPrismatic< axis > >
struct  JointDataPrismatic
struct  JointModelPrismatic
struct  traits< MotionRevoluteUnaligned >
struct  MotionRevoluteUnaligned
struct  traits< ConstraintRevoluteUnaligned >
struct  ConstraintRevoluteUnaligned
struct  traits< JointRevoluteUnaligned >
struct  traits< JointDataRevoluteUnaligned >
struct  traits< JointModelRevoluteUnaligned >
struct  JointDataRevoluteUnaligned
struct  JointModelRevoluteUnaligned
struct  traits< MotionRevolute< axis > >
struct  MotionRevolute
struct  traits< ConstraintRevolute< axis > >
struct  ConstraintRevolute
struct  JointRevolute
struct  traits< JointRevolute< axis > >
struct  traits< JointDataRevolute< axis > >
struct  traits< JointModelRevolute< axis > >
struct  JointDataRevolute
struct  JointModelRevolute
struct  JointSphericalZYXTpl
struct  traits< JointSphericalZYX >
struct  traits< JointDataSphericalZYX >
struct  traits< JointModelSphericalZYX >
struct  JointDataSphericalZYX
struct  JointModelSphericalZYX
struct  traits< MotionSpherical >
struct  MotionSpherical
struct  traits< struct ConstraintRotationalSubspace >
struct  ConstraintRotationalSubspace
struct  traits< JointSpherical >
struct  traits< JointDataSpherical >
struct  traits< JointModelSpherical >
struct  JointDataSpherical
struct  JointModelSpherical
struct  traits< MotionTranslation >
struct  MotionTranslation
struct  traits< ConstraintTranslationSubspace >
struct  ConstraintTranslationSubspace
struct  traits< JointTranslation >
struct  traits< JointDataTranslation >
struct  traits< JointModelTranslation >
struct  JointDataTranslation
struct  JointModelTranslation
class  Model
class  Data
struct  TriangleAndVertices
class  ForceBase
struct  traits< ForceTpl< T, U > >
class  ForceTpl
struct  Frame
 A Plucker coordinate frame attached to a parent joint inside a kinematic tree. More...
struct  traits
class  InertiaBase
struct  traits< InertiaTpl< T, U > >
class  InertiaTpl
class  MotionBase
struct  traits< MotionTpl< T, U > >
class  MotionTpl
struct  traits< BiasZero >
struct  BiasZero
class  SE3Base
 The rigid transform aMb can be seen in two ways: More...
struct  traits< SE3Tpl< T, U > >
class  SE3Tpl
class  Symmetric3Tpl

Typedefs

typedef ConstraintTpl
< 1, double, 0 > 
Constraint1d
typedef ConstraintTpl
< 3, double, 0 > 
Constraint3d
typedef ConstraintTpl
< 6, double, 0 > 
Constraint6d
typedef ConstraintTpl
< Eigen::Dynamic, double, 0 > 
ConstraintXd
typedef ForceSetTpl< double, 0 > ForceSet
typedef std::size_t Index
typedef Index JointIndex
typedef Index GeomIndex
typedef Index FrameIndex
typedef JointPrismatic< 0 > JointPX
typedef JointDataPrismatic< 0 > JointDataPX
typedef JointModelPrismatic< 0 > JointModelPX
typedef JointPrismatic< 1 > JointPY
typedef JointDataPrismatic< 1 > JointDataPY
typedef JointModelPrismatic< 1 > JointModelPY
typedef JointPrismatic< 2 > JointPZ
typedef JointDataPrismatic< 2 > JointDataPZ
typedef JointModelPrismatic< 2 > JointModelPZ
typedef JointRevolute< 0 > JointRX
typedef JointDataRevolute< 0 > JointDataRX
typedef JointModelRevolute< 0 > JointModelRX
typedef JointRevolute< 1 > JointRY
typedef JointDataRevolute< 1 > JointDataRY
typedef JointModelRevolute< 1 > JointModelRY
typedef JointRevolute< 2 > JointRZ
typedef JointDataRevolute< 2 > JointDataRZ
typedef JointModelRevolute< 2 > JointModelRZ
typedef JointSphericalZYXTpl
< double, 0 > 
JointSphericalZYX
typedef boost::variant
< JointModelRX, JointModelRY,
JointModelRZ,
JointModelRevoluteUnaligned,
JointModelSpherical,
JointModelSphericalZYX,
JointModelPX, JointModelPY,
JointModelPZ,
JointModelPrismaticUnaligned,
JointModelFreeFlyer,
JointModelPlanar,
JointModelTranslation,
JointModelDense<-1,-1 > > 
JointModelVariant
typedef boost::variant
< JointDataRX, JointDataRY,
JointDataRZ,
JointDataRevoluteUnaligned,
JointDataSpherical,
JointDataSphericalZYX,
JointDataPX, JointDataPY,
JointDataPZ,
JointDataPrismaticUnaligned,
JointDataFreeFlyer,
JointDataPlanar,
JointDataTranslation,
JointDataDense<-1,-1 > > 
JointDataVariant
typedef std::vector
< JointModelVariant
JointModelVector
typedef std::vector
< JointDataVariant
JointDataVector
typedef fcl::BVHModel
< fcl::OBBRSS > 
PolyhedronType
typedef boost::shared_ptr
< PolyhedronType
Polyhedron_ptr
typedef ForceTpl< double, 0 > Force
typedef InertiaTpl< double, 0 > Inertia
typedef MotionTpl< double, 0 > Motion
typedef SE3Tpl< double, 0 > SE3
typedef Symmetric3Tpl< double, 0 > Symmetric3

Enumerations

enum  GeometryType {
  VISUAL,
  COLLISION,
  NONE
}
enum  { MAX_JOINT_NV = 6 }
enum  ModelFileExtensionType {
  UNKNOWN = 0,
  URDF,
  LUA
}
 Supported model file extensions. More...

Functions

const Eigen::VectorXd & aba (const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Eigen::VectorXd &tau)
 The Articulated-Body algorithm.
const SE3::Vector3centerOfMass (const Model &model, Data &data, const Eigen::VectorXd &q, const bool computeSubtreeComs=true, const bool updateKinematics=true)
 Computes the center of mass position of a given model according to a particular joint configuration.
const SE3::Vector3centerOfMass (const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const bool computeSubtreeComs=true, const bool updateKinematics=true)
 Computes the center of mass position, velocity and acceleration of a given model according to a particular joint configuration, velocity and acceleration.
const SE3::Vector3centerOfMass (const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Eigen::VectorXd &a, const bool computeSubtreeComs=true, const bool updateKinematics=true)
 Computes the center of mass position, velocity and acceleration of a given model according to a particular joint configuration, velocity and acceleration.
const Data::Matrix3xjacobianCenterOfMass (const Model &model, Data &data, const Eigen::VectorXd &q, const bool computeSubtreeComs=true)
 Computes both the jacobian and the the center of mass position of a given model according to a particular joint configuration.
const SE3::Vector3getComFromCrba (const Model &model, Data &data)
 Extracts the center of mass position from the joint space inertia matrix (also called the mass matrix).
const Data::Matrix3xgetJacobianComFromCrba (const Model &model, Data &data)
 Extracts both the jacobian of the center of mass (CoM) and the CoM position from the joint space inertia matrix (also called the mass matrix).
void updateGeometryPlacements (const Model &model, Data &data, const GeometryModel &geom, GeometryData &geom_data, const Eigen::VectorXd &q)
 Apply a forward kinematics and update the placement of the geometry objects (both collision's and visual's one).
void updateGeometryPlacements (const Model &model, const Data &data, const GeometryModel &geom, GeometryData &geom_data)
 Update the placement of the geometry objects according to the current joint placements contained in data.
bool computeCollisions (const Model &model, Data &data, const GeometryModel &model_geom, GeometryData &data_geom, const Eigen::VectorXd &q, const bool stopAtFirstCollision=false)
bool computeCollisions (GeometryData &data_geom, const bool stopAtFirstCollision=false)
void computeDistances (GeometryData &data_geom)
void computeDistances (const Model &model, Data &data, const GeometryModel &model_geom, GeometryData &data_geom, const Eigen::VectorXd &q)
void computeAllTerms (const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
 Computes efficiently all the terms needed for dynamic simulation.
const Eigen::MatrixXd & crba (const Model &model, Data &data, const Eigen::VectorXd &q)
 Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid Body Algorithm (Chapter 6, Rigid-Body Dynamics Algorithms, R.
const Data::Matrix6xccrba (const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
 Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid Body Algorithm (Chapter 6, Rigid-Body Dynamics Algorithms, R.
const Eigen::VectorXd & forwardDynamics (const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Eigen::VectorXd &tau, const Eigen::MatrixXd &J, const Eigen::VectorXd &gamma, const bool updateKinematics=true)
 Compute the forward dynamics with contact constraints.
const Eigen::VectorXd & impulseDynamics (const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v_before, const Eigen::MatrixXd &J, const double r_coeff=0., const bool updateKinematics=true)
 Compute the impulse dynamics with contact constraints.
double kineticEnergy (const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const bool update_kinematics=true)
 Computes the kinetic energy of the system.
double potentialEnergy (const Model &model, Data &data, const Eigen::VectorXd &q, const bool update_kinematics=true)
 Computes the potential energy of the system, i.e.
const Data::Matrix6xcomputeJacobians (const Model &model, Data &data, const Eigen::VectorXd &q)
 Computes the full model Jacobian, i.e.
template<bool localFrame>
void getJacobian (const Model &model, const Data &data, const Model::JointIndex jointId, Data::Matrix6x &J)
 Computes the Jacobian of a specific joint frame expressed either in the world frame or in the local frame of the joint.
const Data::Matrix6xjacobian (const Model &model, Data &data, const Eigen::VectorXd &q, const Model::JointIndex jointId)
 Computes the Jacobian of a specific joint frame expressed in the local frame of the joint.
Eigen::VectorXd integrate (const Model &model, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
 Integrate a configuration for the specified model for a tangent vector during one unit time.
Eigen::VectorXd interpolate (const Model &model, const Eigen::VectorXd &q0, const Eigen::VectorXd &q1, const double u)
 Interpolate the model between two configurations.
Eigen::VectorXd differentiate (const Model &model, const Eigen::VectorXd &q0, const Eigen::VectorXd &q1)
 Compute the tangent vector that must be integrated during one unit time to go from q0 to q1.
Eigen::VectorXd distance (const Model &model, const Eigen::VectorXd &q0, const Eigen::VectorXd &q1)
 Distance between two configuration vectors.
Eigen::VectorXd randomConfiguration (const Model &model, const Eigen::VectorXd &lowerLimits, const Eigen::VectorXd &upperLimits)
 Generate a configuration vector uniformly sampled among provided limits.
Eigen::VectorXd randomConfiguration (const Model &model)
 Generate a configuration vector uniformly sampled among the joint limits of the specified Model.
void emptyForwardPass (const Model &model, Data &data)
 Browse through the kinematic structure with a void step.
void forwardKinematics (const Model &model, Data &data, const Eigen::VectorXd &q)
 Update the joint placement according to the current joint configuration.
void forwardKinematics (const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
 Update the joint placement according to the current joint configuration and velocity.
void forwardKinematics (const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Eigen::VectorXd &a)
 Update the joint placement according to the current joint configuration, velocity and acceleration.
void framesForwardKinematics (const Model &model, Data &data)
 Update the position of each extra frame.
void framesForwardKinematics (const Model &model, Data &data, const Eigen::VectorXd &q)
 Compute Kinematics of full model, then the position of each operational frame.
template<bool localFrame>
void getFrameJacobian (const Model &model, const Data &data, const Model::FrameIndex frame_id, Data::Matrix6x &J)
 Return the jacobian of the operational frame in the world frame or in the local frame depending on the template argument.
const Eigen::VectorXd & rnea (const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Eigen::VectorXd &a)
 The Recursive Newton-Euler algorithm.
const Eigen::VectorXd & nonLinearEffects (const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
 Computes the non-linear effects (Corriolis, centrifual and gravitationnal effects), also called the biais terms \( b(q,\dot{q}) \) of the Lagrangian dynamics:

\( \begin{eqnarray} M \ddot{q} + b(q, \dot{q}) = \tau \end{eqnarray} \)



template<typename D >
D::Scalar angleBetweenQuaternions (const Eigen::QuaternionBase< D > &q1, const Eigen::QuaternionBase< D > &q2)
 Compute the minimal angle between q1 and q2.
template<typename Derived , typename otherDerived >
bool defineSameRotation (const Eigen::QuaternionBase< Derived > &q1, const Eigen::QuaternionBase< otherDerived > &q2)
 Check if two quaternions define the same rotations.
bool operator== (const fcl::CollisionObject &lhs, const fcl::CollisionObject &rhs)
 Return true if the intrinsic geometry of the two CollisionObject is the same.
bool operator== (const GeometryObject &lhs, const GeometryObject &rhs)
std::ostream & operator<< (std::ostream &os, const GeometryObject &geom_object)
JointDataVariant createData (const JointModelVariant &jmodel)
 Visit a JointModelVariant through CreateData visitor to create a JointDataVariant.
void calc_zero_order (const JointModelVariant &jmodel, JointDataVariant &jdata, const Eigen::VectorXd &q)
 Visit a JointDataVariant and the corresponding JointModelVariant through JointCalcZeroOrderVisitor to compute the joint data kinematics at order zero.
void calc_first_order (const JointModelVariant &jmodel, JointDataVariant &jdata, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
 Visit a JointDataVariant and the corresponding JointModelVariant through JointCalcFirstOrderVisitor to compute the joint data kinematics at order one.
void calc_aba (const JointModelVariant &jmodel, JointDataVariant &jdata, Inertia::Matrix6 &I, const bool update_I)
 Visit a JointDataVariant and the corresponding JointModelVariant through JointCalcAbaVisitor to.
Eigen::VectorXd integrate (const JointModelVariant &jmodel, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
 Visit a JointModelVariant through JointIntegrateVisitor to integrate joint's configuration for a tangent vector during one unit time.
Eigen::VectorXd interpolate (const JointModelVariant &jmodel, const Eigen::VectorXd &q0, const Eigen::VectorXd &q1, const double u)
 Visit a JointModelVariant through JointInterpolateVisitor to compute the interpolation between two joint's configurations.
Eigen::VectorXd randomConfiguration (const JointModelVariant &jmodel, const Eigen::VectorXd &lower_pos_limit, const Eigen::VectorXd &upper_pos_limit)
 Visit a JointModelVariant through JointRandomConfigurationVisitor to generate a configuration vector uniformly sampled among provided limits.
Eigen::VectorXd difference (const JointModelVariant &jmodel, const Eigen::VectorXd &q0, const Eigen::VectorXd &q1)
 Visit a JointModelVariant through JointDifferenceVisitor to compute the tangent vector that must be integrated during one unit time to go from q0 to q1.
double distance (const JointModelVariant &jmodel, const Eigen::VectorXd &q0, const Eigen::VectorXd &q1)
 Visit a JointModelVariant through JointDifferenceVisitor to compute the distance between two configurations.
int nv (const JointModelVariant &jmodel)
 Visit a JointModelVariant through JointNvVisitor to get the dimension of the joint tangent space.
int nq (const JointModelVariant &jmodel)
 Visit a JointModelVariant through JointNqVisitor to get the dimension of the joint configuration space.
int idx_q (const JointModelVariant &jmodel)
 Visit a JointModelVariant through JointIdxQVisitor to get the index in the full model configuration space corresponding to the first degree of freedom of the Joint.
int idx_v (const JointModelVariant &jmodel)
 Visit a JointModelVariant through JointIdxVVisitor to get the index in the full model tangent space corresponding to the first joint tangent space degree.
JointIndex id (const JointModelVariant &jmodel)
 Visit a JointModelVariant through JointIdVisitor to get the index of the joint in the kinematic chain.
void setIndexes (JointModelVariant &jmodel, JointIndex id, int q, int v)
 Visit a JointModelVariant through JointSetIndexesVisitor to set the indexes of the joint in the kinematic chain.
ConstraintXd constraint_xd (const JointDataVariant &jdata)
 Visit a JointDataVariant through JointConstraintVisitor to get the joint constraint as a dense constraint.
SE3 joint_transform (const JointDataVariant &jdata)
 Visit a JointDataVariant through JointTransformVisitor to get the joint internal transform (transform between the entry frame and the exit frame of the joint)
Motion motion (const JointDataVariant &jdata)
 Visit a JointDataVariant through JointMotionVisitor to get the joint internal motion as a dense motion.
Motion bias (const JointDataVariant &jdata)
 Visit a JointDataVariant through JointBiasVisitor to get the joint bias as a dense motion.
Eigen::Matrix< double,
6, Eigen::Dynamic > 
u_inertia (const JointDataVariant &jdata)
 Visit a JointDataVariant through JointUInertiaVisitor to get the U matrix of the inertia matrix decomposition.
Eigen::Matrix< double,
Eigen::Dynamic, Eigen::Dynamic > 
dinv_inertia (const JointDataVariant &jdata)
 Visit a JointDataVariant through JointDInvInertiaVisitor to get the D^{-1} matrix of the inertia matrix decomposition.
Eigen::Matrix< double,
6, Eigen::Dynamic > 
udinv_inertia (const JointDataVariant &jdata)
 Visit a JointDataVariant through JointUDInvInertiaVisitor to get U*D^{-1} matrix of the inertia matrix decomposition.
template<typename D >
Motion operator* (const ConstraintIdentity &, const Eigen::MatrixBase< D > &v)
Inertia::Matrix6 operator* (const Inertia &Y, const ConstraintIdentity &)
const Inertia::Matrix6 & operator* (const Inertia::Matrix6 &Y, const ConstraintIdentity &)
Inertia::Matrix6 & operator* (Inertia::Matrix6 &Y, const ConstraintIdentity &)
Inertia::Matrix6 operator* (const ConstraintIdentity::TransposeConst &, const Inertia &Y)
template<typename D >
const Eigen::MatrixBase< D > & operator* (const ConstraintIdentity::TransposeConst &, const Eigen::MatrixBase< D > &F)
const MotionPlanar operator+ (const MotionPlanar &m, const BiasZero &)
Motion operator+ (const MotionPlanar &m1, const Motion &m2)
template<typename D >
Motion operator* (const ConstraintPlanar &, const Eigen::MatrixBase< D > &v)
Motion operator^ (const Motion &m1, const MotionPlanar &m2)
Eigen::Matrix
< Inertia::Scalar_t, 6, 3 > 
operator* (const Inertia &Y, const ConstraintPlanar &)
Eigen::Matrix
< Inertia::Scalar_t, 6, 3 > 
operator* (const Inertia::Matrix6 &Y, const ConstraintPlanar &)
const MotionPrismaticUnalignedoperator+ (const MotionPrismaticUnaligned &m, const BiasZero &)
Motion operator+ (const MotionPrismaticUnaligned &m1, const Motion &m2)
Motion operator^ (const Motion &m1, const MotionPrismaticUnaligned &m2)
Eigen::Matrix< double, 6, 1 > operator* (const Inertia &Y, const ConstraintPrismaticUnaligned &cpu)
Eigen::ProductReturnType
< Eigen::Block< const
Inertia::Matrix6, 6, 3 >
, const
ConstraintPrismaticUnaligned::Vector3 >
::Type 
operator* (const Inertia::Matrix6 &Y, const ConstraintPrismaticUnaligned &cpu)
template<int axis>
const MotionPrismatic< axis > & operator+ (const MotionPrismatic< axis > &m, const BiasZero &)
template<int axis>
Motion operator+ (const MotionPrismatic< axis > &m1, const Motion &m2)
Motion operator^ (const Motion &m1, const MotionPrismatic< 0 > &m2)
Motion operator^ (const Motion &m1, const MotionPrismatic< 1 > &m2)
Motion operator^ (const Motion &m1, const MotionPrismatic< 2 > &m2)
Eigen::Matrix< double, 6, 1 > operator* (const Inertia &Y, const ConstraintPrismatic< 0 > &)
Eigen::Matrix< double, 6, 1 > operator* (const Inertia &Y, const ConstraintPrismatic< 1 > &)
Eigen::Matrix< double, 6, 1 > operator* (const Inertia &Y, const ConstraintPrismatic< 2 > &)
template<int axis>
const Eigen::MatrixBase< const
Inertia::Matrix6 >::ColXpr 
operator* (const Inertia::Matrix6 &Y, const ConstraintPrismatic< axis > &)
const MotionRevoluteUnalignedoperator+ (const MotionRevoluteUnaligned &m, const BiasZero &)
Motion operator+ (const MotionRevoluteUnaligned &m1, const Motion &m2)
Motion operator^ (const Motion &m1, const MotionRevoluteUnaligned &m2)
Eigen::Matrix< double, 6, 1 > operator* (const Inertia &Y, const ConstraintRevoluteUnaligned &cru)
Eigen::ProductReturnType
< Eigen::Block< const
Inertia::Matrix6, 6, 3 >
, const
ConstraintRevoluteUnaligned::Vector3 >
::Type 
operator* (const Inertia::Matrix6 &Y, const ConstraintRevoluteUnaligned &cru)
template<int axis>
const MotionRevolute< axis > & operator+ (const MotionRevolute< axis > &m, const BiasZero &)
template<int axis>
Motion operator+ (const MotionRevolute< axis > &m1, const Motion &m2)
Motion operator^ (const Motion &m1, const MotionRevolute< 0 > &m2)
Motion operator^ (const Motion &m1, const MotionRevolute< 1 > &m2)
Motion operator^ (const Motion &m1, const MotionRevolute< 2 > &m2)
Eigen::Matrix< double, 6, 1 > operator* (const Inertia &Y, const ConstraintRevolute< 0 > &)
Eigen::Matrix< double, 6, 1 > operator* (const Inertia &Y, const ConstraintRevolute< 1 > &)
Eigen::Matrix< double, 6, 1 > operator* (const Inertia &Y, const ConstraintRevolute< 2 > &)
template<int axis>
const Eigen::MatrixBase< const
Inertia::Matrix6 >::ColXpr 
operator* (const Inertia::Matrix6 &Y, const ConstraintRevolute< axis > &)
template<int axis>
Eigen::MatrixBase
< Inertia::Matrix6 >::ColXpr 
operator* (Inertia::Matrix6 &Y, const ConstraintRevolute< axis > &)
Motion operator^ (const Motion &m1, const JointSphericalZYX::MotionSpherical &m2)
template<typename _Scalar , int _Options>
Eigen::Matrix< _Scalar,
6, 3, _Options > 
operator* (const InertiaTpl< _Scalar, _Options > &Y, const typename JointSphericalZYXTpl< _Scalar, _Options >::ConstraintRotationalSubspace &S)
template<typename _Scalar , int _Options>
Eigen::ProductReturnType
< const Eigen::Block< const
Inertia::Matrix6, 6, 3 >
, const typename
JointSphericalZYXTpl< _Scalar,
_Options >
::ConstraintRotationalSubspace::Matrix3 >
::Type 
operator* (const typename InertiaTpl< _Scalar, _Options >::Matrix6 &Y, const typename JointSphericalZYXTpl< _Scalar, _Options >::ConstraintRotationalSubspace &S)
const MotionSpherical operator+ (const MotionSpherical &m, const BiasZero &)
Motion operator+ (const MotionSpherical &m1, const Motion &m2)
template<typename D >
Motion operator* (const ConstraintRotationalSubspace &, const Eigen::MatrixBase< D > &v)
Motion operator^ (const Motion &m1, const MotionSpherical &m2)
Eigen::Matrix< double, 6, 3 > operator* (const Inertia &Y, const ConstraintRotationalSubspace &)
const MotionTranslation operator+ (const MotionTranslation &m, const BiasZero &)
Motion operator+ (const MotionTranslation &m1, const Motion &m2)
template<typename D >
Motion operator* (const ConstraintTranslationSubspace &, const Eigen::MatrixBase< D > &v)
Motion operator^ (const Motion &m1, const MotionTranslation &m2)
Eigen::Matrix< double, 6, 3 > operator* (const Inertia &Y, const ConstraintTranslationSubspace &)
void buildMesh (const ::urdf::Vector3 &scale, const aiScene *scene, const aiNode *node, std::vector< unsigned > &subMeshIndexes, const Polyhedron_ptr &mesh, TriangleAndVertices &tv)
 Recursive procedure for building a mesh.
void meshFromAssimpScene (const std::string &name, const ::urdf::Vector3 &scale, const aiScene *scene, const Polyhedron_ptr &mesh) throw (std::invalid_argument)
 Convert an assimp scene to a mesh.
void loadPolyhedronFromResource (const std::string &resource_path, const ::urdf::Vector3 &scale, const Polyhedron_ptr &polyhedron) throw (std::invalid_argument)
 Read a mesh file and convert it to a polyhedral mesh.
std::string convertURDFMeshPathToAbsolutePath (const std::string &urdf_mesh_path, const std::vector< std::string > &package_dirs)
 Transform a package://.
ModelFileExtensionType checkModelFileExtension (const std::string &filename)
 Extract the type of the given model file according to its extension.
template<typename D >
Eigen::Matrix< typename
D::Scalar,
3, 3, Eigen::internal::traits
< D >::Options > 
exp3 (const Eigen::MatrixBase< D > &v)
 Exp: so3 -> SO3.
template<typename D >
Eigen::Matrix< typename
D::Scalar,
3, 1, Eigen::internal::traits
< D >::Options > 
log3 (const Eigen::MatrixBase< D > &R)
 Log: SO3 -> so3.
template<typename _Scalar , int _Options>
SE3Tpl< _Scalar, _Options > exp6 (const MotionTpl< _Scalar, _Options > &nu)
 Exp: se3 -> SE3.
template<typename D >
SE3Tpl< typename D::Scalar,
Eigen::internal::traits< D >
::Options > 
exp6 (const Eigen::MatrixBase< D > &v)
 Exp: se3 -> SE3.
template<typename _Scalar , int _Options>
MotionTpl< _Scalar, _Options > log6 (const SE3Tpl< _Scalar, _Options > &M)
 Log: SE3 -> se3.
template<typename D >
MotionTpl< typename D::Scalar,
Eigen::internal::traits< D >
::Options > 
log6 (const Eigen::MatrixBase< D > &M)
 Log: SE3 -> se3.
fcl::Matrix3f toFclMatrix3f (const Eigen::Matrix3d &mat)
Eigen::Matrix3d toMatrix3d (const fcl::Matrix3f &mat)
fcl::Vec3f toFclVec3f (const Eigen::Vector3d &vec)
Eigen::Vector3d toVector3d (const fcl::Vec3f &vec)
fcl::Transform3f toFclTransform3f (const se3::SE3 &m)
se3::SE3 toPinocchioSE3 (const fcl::Transform3f &tf)
template<typename S , int O>
MotionTpl< S, O > operator^ (const MotionTpl< S, O > &m1, const MotionTpl< S, O > &m2)
template<typename S , int O>
ForceTpl< S, O > operator^ (const MotionTpl< S, O > &m, const ForceTpl< S, O > &f)
const Motionoperator+ (const Motion &v, const BiasZero &)
const Motionoperator+ (const BiasZero &, const Motion &v)
template<typename D >
Eigen::Matrix< typename
D::Scalar, 3, 3, D::Options > 
skew (const Eigen::MatrixBase< D > &v)
 Computes the skew representation of a given 3D vector, i.e.
template<typename D >
Eigen::Matrix< typename
D::Scalar, 3, 1, D::Options > 
unSkew (const Eigen::MatrixBase< D > &M)
 Inverse operation related to skew.
template<typename D >
Eigen::Matrix< typename
D::Scalar, 3, 3, D::Options > 
alphaSkew (const typename D::Scalar s, const Eigen::MatrixBase< D > &v)
template<typename V , typename M >
Eigen::Matrix< typename
M::Scalar,
3, M::ColsAtCompileTime,
M::Options > 
cross (const Eigen::MatrixBase< V > &v, const Eigen::MatrixBase< M > &m)
std::vector< std::string > extractPathFromEnvVar (const std::string &env_var_name, const std::string &delimiter=":")
 Parse an environment variable if exists and extract paths according to the delimiter.
std::string random (const int len)

Variables

const double PI = boost::math::constants::pi<double>()

Typedef Documentation

typedef ConstraintTpl<1,double,0> se3::Constraint1d
typedef ConstraintTpl<3,double,0> se3::Constraint3d
typedef ConstraintTpl<6,double,0> se3::Constraint6d
typedef ConstraintTpl<Eigen::Dynamic,double,0> se3::ConstraintXd
typedef ForceTpl<double,0> se3::Force
typedef ForceSetTpl<double,0> se3::ForceSet
typedef std::size_t se3::Index
typedef InertiaTpl<double,0> se3::Inertia
typedef std::vector<JointDataVariant> se3::JointDataVector
typedef MotionTpl<double,0> se3::Motion
typedef boost::shared_ptr<PolyhedronType> se3::Polyhedron_ptr
typedef fcl::BVHModel< fcl::OBBRSS > se3::PolyhedronType
typedef SE3Tpl<double,0> se3::SE3
typedef Symmetric3Tpl<double,0> se3::Symmetric3

Enumeration Type Documentation

anonymous enum
Enumerator:
MAX_JOINT_NV 
Enumerator:
VISUAL 
COLLISION 
NONE 

Supported model file extensions.

Enumerator:
UNKNOWN 
URDF 
LUA 

Function Documentation

const Eigen::VectorXd& se3::aba ( const Model &  model,
Data &  data,
const Eigen::VectorXd &  q,
const Eigen::VectorXd &  v,
const Eigen::VectorXd &  tau 
) [inline]

The Articulated-Body algorithm.

It computes the forward dynamics, aka the joint accelerations given the current state and actuation of the model.

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]qThe joint configuration vector (dim model.nq).
[in]vThe joint velocity vector (dim model.nv).
[in]tauThe joint torque vector (dim model.nv).
Returns:
The current joint acceleration stored in data.ddq.

Referenced by se3::python::AlgorithmsPythonVisitor::aba_proxy().

template<typename D >
Eigen::Matrix<typename D::Scalar,3,3,D::Options> se3::alphaSkew ( const typename D::Scalar  s,
const Eigen::MatrixBase< D > &  v 
) [inline]

Referenced by exp6(), log6(), and operator*().

template<typename D >
D::Scalar se3::angleBetweenQuaternions ( const Eigen::QuaternionBase< D > &  q1,
const Eigen::QuaternionBase< D > &  q2 
)

Compute the minimal angle between q1 and q2.

Parameters:
[in]q1input quaternion.
[in]q2input quaternion.
Returns:
angle between the two quaternions
Motion se3::bias ( const JointDataVariant &  jdata) [inline]

Visit a JointDataVariant through JointBiasVisitor to get the joint bias as a dense motion.

Parameters:
[in]jdataThe jdata
Returns:
The motion dense corresponding to the joint derived bias

Referenced by se3::JointDataAccessor::c().

void se3::buildMesh ( const ::urdf::Vector3 scale,
const aiScene *  scene,
const aiNode *  node,
std::vector< unsigned > &  subMeshIndexes,
const Polyhedron_ptr &  mesh,
TriangleAndVertices &  tv 
) [inline]

Recursive procedure for building a mesh.

Parameters:
[in]scaleScale to apply when reading the ressource
[in]scenePointer to the assimp scene
[in]nodeCurrent node of the scene
subMeshIndexesSubmesh triangles indexes interval
[in]meshThe mesh that must be built
tvTriangles and Vertices of the mesh submodels

References se3::TriangleAndVertices::triangles_, and se3::TriangleAndVertices::vertices_.

Referenced by meshFromAssimpScene().

void se3::calc_aba ( const JointModelVariant &  jmodel,
JointDataVariant &  jdata,
Inertia::Matrix6 &  I,
const bool  update_I 
) [inline]

Visit a JointDataVariant and the corresponding JointModelVariant through JointCalcAbaVisitor to.

Parameters:
[in]jmodelThe corresponding JointModelVariant to the JointDataVariant we want to update
jdataThe JointDataVariant we want to update
IInertia matrix of the subtree following the jmodel in the kinematic chain as dense matrix
[in]update_IIf I should be updated or not
void se3::calc_first_order ( const JointModelVariant &  jmodel,
JointDataVariant &  jdata,
const Eigen::VectorXd &  q,
const Eigen::VectorXd &  v 
) [inline]

Visit a JointDataVariant and the corresponding JointModelVariant through JointCalcFirstOrderVisitor to compute the joint data kinematics at order one.

Parameters:
[in]jmodelThe corresponding JointModelVariant to the JointDataVariant we want to update
jdataThe JointDataVariant we want to update
[in]qThe full model's (in which the joint belongs to) configuration vector

Referenced by se3::JointModelAccessor::calc().

void se3::calc_zero_order ( const JointModelVariant &  jmodel,
JointDataVariant &  jdata,
const Eigen::VectorXd &  q 
) [inline]

Visit a JointDataVariant and the corresponding JointModelVariant through JointCalcZeroOrderVisitor to compute the joint data kinematics at order zero.

Parameters:
[in]jmodelThe corresponding JointModelVariant to the JointDataVariant we want to update
jdataThe JointDataVariant we want to update
[in]qThe full model's (in which the joint belongs to) configuration vector

Referenced by se3::JointModelAccessor::calc().

const Data::Matrix6x& se3::ccrba ( const Model &  model,
Data &  data,
const Eigen::VectorXd &  q,
const Eigen::VectorXd &  v 
) [inline]

Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid Body Algorithm (Chapter 6, Rigid-Body Dynamics Algorithms, R.

Featherstone, 2008). The result is accessible through data.M.

Note:
You can easly get data.M symetric by copying the stricly upper trinangular part in the stricly lower tringular part with data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]qThe joint configuration vector (dim model.nq).
[in]vThe joint configuration vector (dim model.nv).
Returns:
The joint space inertia matrix with only the upper triangular part computed.

Referenced by se3::python::AlgorithmsPythonVisitor::ccrba_proxy().

const SE3::Vector3& se3::centerOfMass ( const Model &  model,
Data &  data,
const Eigen::VectorXd &  q,
const bool  computeSubtreeComs = true,
const bool  updateKinematics = true 
) [inline]

Computes the center of mass position of a given model according to a particular joint configuration.

The result is accessible through data.com[0] for the full body com and data.com[i] for the subtree supported by joint i (expressed in the joint i frame).

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]qThe joint configuration vector (dim model.nq).
[in]computeSubtreeComsIf true, the algorithm computes also the center of mass of the subtrees.
[in]updateKinematicsIf true, the algorithm updates first the geometry of the tree. Otherwise, it uses the current kinematics stored in data.
Returns:
The center of mass position of the full rigid body system expressed in the world frame.

Referenced by se3::python::AlgorithmsPythonVisitor::com_0_proxy(), se3::python::AlgorithmsPythonVisitor::com_1_proxy(), and se3::python::AlgorithmsPythonVisitor::com_2_proxy().

const SE3::Vector3& se3::centerOfMass ( const Model &  model,
Data &  data,
const Eigen::VectorXd &  q,
const Eigen::VectorXd &  v,
const bool  computeSubtreeComs = true,
const bool  updateKinematics = true 
) [inline]

Computes the center of mass position, velocity and acceleration of a given model according to a particular joint configuration, velocity and acceleration.

The result is accessible through data.com[0], data.vcom[0], data.acom[0] for the full body com position, velocity and acceleation. And data.com[i], data.vcom[i] and data.acom[i] for the subtree supported by joint i (expressed in the joint i frame).

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]qThe joint configuration vector (dim model.nq).
[in]vThe joint velocity vector (dim model.nv).
[in]computeSubtreeComsIf true, the algorithm computes also the center of mass of the subtrees.
[in]updateKinematicsIf true, the algorithm updates first the second order kinematics of the tree. Otherwise, it uses the current kinematics stored in data.
Returns:
The center of mass position of the full rigid body system expressed in the world frame.
const SE3::Vector3& se3::centerOfMass ( const Model &  model,
Data &  data,
const Eigen::VectorXd &  q,
const Eigen::VectorXd &  v,
const Eigen::VectorXd &  a,
const bool  computeSubtreeComs = true,
const bool  updateKinematics = true 
) [inline]

Computes the center of mass position, velocity and acceleration of a given model according to a particular joint configuration, velocity and acceleration.

The result is accessible through data.com[0], data.vcom[0], data.acom[0] for the full body com position, velocity and acceleation. And data.com[i], data.vcom[i] and data.acom[i] for the subtree supported by joint i (expressed in the joint i frame).

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]qThe joint configuration vector (dim model.nq).
[in]vThe joint velocity vector (dim model.nv).
[in]aThe joint acceleration vector (dim model.nv).
[in]computeSubtreeComsIf true, the algorithm computes also the center of mass of the subtrees.
[in]updateKinematicsIf true, the algorithm updates first the second order kinematics of the tree. Otherwise, it uses the current kinematics stored in data.
Returns:
The center of mass position of the full rigid body system expressed in the world frame.
ModelFileExtensionType se3::checkModelFileExtension ( const std::string &  filename)

Extract the type of the given model file according to its extension.

Parameters:
[in]filemaneThe complete path to the model file.
Returns:
The type of the extension of the model file

References LUA, UNKNOWN, and URDF.

void se3::computeAllTerms ( const Model model,
Data data,
const Eigen::VectorXd &  q,
const Eigen::VectorXd &  v 
) [inline]

Computes efficiently all the terms needed for dynamic simulation.

It is equivalent to the call at the same time to:

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]qThe joint configuration vector (dim model.nq).
[in]vThe joint velocity vector (dim model.nv).
Returns:
All the results are stored in data. Please refer to the specific algorithm for further details.

References se3::Data::a, se3::Data::a_gf, se3::Data::com, se3::Model::gravity, se3::Data::Jcom, se3::Model::joints, se3::Data::joints, kineticEnergy(), se3::Data::mass, se3::Model::nbody, potentialEnergy(), se3::fusion::JointVisitor< CATForwardStep >::run(), se3::Data::v, and se3::Data::vcom.

Referenced by se3::python::AlgorithmsPythonVisitor::computeAllTerms_proxy(), and forwardDynamics().

bool se3::computeCollisions ( const Model &  model,
Data &  data,
const GeometryModel &  model_geom,
GeometryData &  data_geom,
const Eigen::VectorXd &  q,
const bool  stopAtFirstCollision = false 
) [inline]
bool se3::computeCollisions ( GeometryData &  data_geom,
const bool  stopAtFirstCollision = false 
) [inline]
void se3::computeDistances ( GeometryData &  data_geom) [inline]
void se3::computeDistances ( const Model &  model,
Data &  data,
const GeometryModel &  model_geom,
GeometryData &  data_geom,
const Eigen::VectorXd &  q 
) [inline]
const Data::Matrix6x& se3::computeJacobians ( const Model &  model,
Data &  data,
const Eigen::VectorXd &  q 
) [inline]

Computes the full model Jacobian, i.e.

the stack of all motion subspace expressed in the world frame. The result is accessible through data.J.

Note:
This Jacobian does not correspond to any specific joint frame Jacobian. From this Hacobian, it is then possible to easily extract the Jacobian of a specific joint frame.
Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]qThe joint configuration vector (dim model.nq).
Returns:
The full model Jacobian (matrix 6 x model.nv).

Referenced by se3::python::AlgorithmsPythonVisitor::compute_jacobians_proxy(), se3::python::AlgorithmsPythonVisitor::frame_jacobian_proxy(), and se3::python::AlgorithmsPythonVisitor::jacobian_proxy().

ConstraintXd se3::constraint_xd ( const JointDataVariant &  jdata) [inline]

Visit a JointDataVariant through JointConstraintVisitor to get the joint constraint as a dense constraint.

Parameters:
[in]jdataThe jdata
Returns:
The constraint dense corresponding to the joint derived constraint

Referenced by se3::JointDataAccessor::S().

std::string se3::convertURDFMeshPathToAbsolutePath ( const std::string &  urdf_mesh_path,
const std::vector< std::string > &  package_dirs 
) [inline]

Transform a package://.

. mesh path to an absolute path, searching for a valid file in a list of package directories

Parameters:
[in]urdf_mesh_pathThe path given in the urdf file
[in]package_dirsA list of packages directories where to search for meshes
Returns:
The absolute path to the mesh file

Referenced by se3::urdf::retrieveCollisionGeometry().

const Eigen::MatrixXd& se3::crba ( const Model &  model,
Data &  data,
const Eigen::VectorXd &  q 
) [inline]

Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid Body Algorithm (Chapter 6, Rigid-Body Dynamics Algorithms, R.

Featherstone, 2008). The result is accessible through data.M.

Note:
You can easly get data.M symetric by copying the stricly upper trinangular part in the stricly lower tringular part with data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]qThe joint configuration vector (dim model.nq).
Returns:
The joint space inertia matrix with only the upper triangular part computed.

Referenced by se3::python::AlgorithmsPythonVisitor::crba_proxy(), and impulseDynamics().

JointDataVariant se3::createData ( const JointModelVariant &  jmodel) [inline]

Visit a JointModelVariant through CreateData visitor to create a JointDataVariant.

Parameters:
[in]jmodelThe JointModelVariant we want to create a data for
Returns:
The created JointDataVariant
template<typename V , typename M >
Eigen::Matrix<typename M::Scalar,3,M::ColsAtCompileTime,M::Options> se3::cross ( const Eigen::MatrixBase< V > &  v,
const Eigen::MatrixBase< M > &  m 
) [inline]
template<typename Derived , typename otherDerived >
bool se3::defineSameRotation ( const Eigen::QuaternionBase< Derived > &  q1,
const Eigen::QuaternionBase< otherDerived > &  q2 
)

Check if two quaternions define the same rotations.

Note:
Two quaternions define the same rotation iff q1 == q2 OR q1 == -q2.
Parameters:
[in]q1input quaternion.
[in]q2input quaternion.
Returns:
Return true if the two input quaternions define the same rotation.
Eigen::VectorXd se3::difference ( const JointModelVariant &  jmodel,
const Eigen::VectorXd &  q0,
const Eigen::VectorXd &  q1 
) [inline]

Visit a JointModelVariant through JointDifferenceVisitor to compute the tangent vector that must be integrated during one unit time to go from q0 to q1.

Parameters:
[in]jmodelThe JointModelVariant
[in]q0Initial configuration
[in]q1Wished configuration
Returns:
The corresponding velocity
Eigen::VectorXd se3::differentiate ( const Model &  model,
const Eigen::VectorXd &  q0,
const Eigen::VectorXd &  q1 
) [inline]

Compute the tangent vector that must be integrated during one unit time to go from q0 to q1.

Parameters:
[in]modelModel to be differentiated
[in]q0Initial configuration (size model.nq)
[in]q1Wished configuration (size model.nq)
Returns:
The corresponding velocity (size model.nv)

References se3::Model::joints, se3::Model::nbody, se3::Model::nv, and se3::fusion::JointModelVisitor< DifferentiateStep >::run().

Referenced by se3::python::AlgorithmsPythonVisitor::differentiate_proxy().

Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> se3::dinv_inertia ( const JointDataVariant &  jdata) [inline]

Visit a JointDataVariant through JointDInvInertiaVisitor to get the D^{-1} matrix of the inertia matrix decomposition.

Parameters:
[in]jdataThe jdata
Returns:
The D^{-1} matrix of the inertia matrix decomposition

Referenced by se3::JointDataAccessor::Dinv().

Eigen::VectorXd se3::distance ( const Model &  model,
const Eigen::VectorXd &  q0,
const Eigen::VectorXd &  q1 
) [inline]

Distance between two configuration vectors.

Parameters:
[in]modelModel we want to compute the distance
[in]q0Configuration 0 (size model.nq)
[in]q1Configuration 1 (size model.nq)
Returns:
The corresponding distances for each joint (size model.nbody-1 = number of joints)

References se3::Model::joints, se3::Model::nbody, and se3::fusion::JointModelVisitor< DistanceStep >::run().

Referenced by se3::python::AlgorithmsPythonVisitor::distance_proxy().

double se3::distance ( const JointModelVariant &  jmodel,
const Eigen::VectorXd &  q0,
const Eigen::VectorXd &  q1 
) [inline]

Visit a JointModelVariant through JointDifferenceVisitor to compute the distance between two configurations.

Parameters:
[in]jmodelThe JointModelVariant
[in]q0Configuration 1
[in]q1Configuration 2
Returns:
The corresponding distance
void se3::emptyForwardPass ( const Model &  model,
Data &  data 
) [inline]

Browse through the kinematic structure with a void step.

Note:
This void step allows to quantify the time spent in the rollout.
Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.

References se3::Model::joints, se3::Data::joints, se3::Model::nbody, and se3::fusion::JointVisitor< emptyForwardStep >::run().

template<typename D >
Eigen::Matrix<typename D::Scalar,3,3,Eigen::internal::traits<D>::Options> se3::exp3 ( const Eigen::MatrixBase< D > &  v)

Exp: so3 -> SO3.

Return the integral of the input angular velocity during time 1.

Parameters:
[in]vThe angular velocity vector.
Returns:
The rotational matrix associated to the integration of the angular velocity during time 1.

References nv().

Referenced by se3::python::ExplogPythonVisitor::exp3_proxy(), exp6(), se3::JointModelFreeFlyer::integrate_impl(), and se3::JointModelSpherical::integrate_impl().

template<typename _Scalar , int _Options>
SE3Tpl<_Scalar, _Options> se3::exp6 ( const MotionTpl< _Scalar, _Options > &  nu)

Exp: se3 -> SE3.

Return the integral of the input twist during time 1.

Parameters:
[in]nuThe input twist.
Returns:
The rigid transformation associated to the integration of the twist during time 1.

References alphaSkew(), se3::MotionBase< MotionTpl< _Scalar, _Options > >::angular(), exp3(), se3::MotionBase< MotionTpl< _Scalar, _Options > >::linear(), and SINCOS.

Referenced by exp6(), se3::python::ExplogPythonVisitor::exp6FromMotion_proxy(), and se3::python::ExplogPythonVisitor::exp6FromVector_proxy().

template<typename D >
SE3Tpl<typename D::Scalar, Eigen::internal::traits<D>::Options> se3::exp6 ( const Eigen::MatrixBase< D > &  v)

Exp: se3 -> SE3.

Return the integral of the input spatial velocity during time 1.

Parameters:
[in]vThe twist represented by a vector.
Returns:
The rigid transformation associated to the integration of the twist vector during time 1..

References exp6().

std::vector<std::string> se3::extractPathFromEnvVar ( const std::string &  env_var_name,
const std::string &  delimiter = ":" 
) [inline]

Parse an environment variable if exists and extract paths according to the delimiter.

Parameters:
[in]env_var_nameThe name of the environment variable.
[in]delimiterThe delimiter between two consecutive paths.
Returns:
The vector of paths extracted from the environment variable value.

Referenced by se3::urdf::buildGeom().

const Eigen::VectorXd& se3::forwardDynamics ( const Model &  model,
Data &  data,
const Eigen::VectorXd &  q,
const Eigen::VectorXd &  v,
const Eigen::VectorXd &  tau,
const Eigen::MatrixXd &  J,
const Eigen::VectorXd &  gamma,
const bool  updateKinematics = true 
) [inline]

Compute the forward dynamics with contact constraints.

Note:
It computes the following problem:
\( \begin{eqnarray} \underset{\ddot{q}}{\min} & & \| \ddot{q} - \ddot{q}_{\text{free}} \|_{M(q)} \\ \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 \end{eqnarray} \)

where \( \ddot{q}_{\text{free}} \) is the free acceleration (i.e. without constraints), \( M \) is the mass matrix, \( J \) the constraint Jacobian and \( \gamma \) is the constraint drift.
Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]qThe joint configuration (vector dim model.nq).
[in]vThe joint velocity (vector dim model.nv).
[in]tauThe joint torque vector (dim model.nv).
[in]JThe Jacobian of the constraints (dim nb_constraints*model.nv).
[in]gammaThe drift of the constraints (dim nb_constraints).
[in]updateKinematicsIf true, the algorithm calls first se3::computeAllTerms. Otherwise, it uses the current dynamic values stored in data.
Returns:
A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers linked to the contact forces are available throw data.lambda_c vector.

References computeAllTerms(), se3::Data::D, se3::Data::ddq, se3::cholesky::decompose(), se3::Data::JMinvJt, se3::Data::lambda_c, se3::Data::llt_JMinvJt, se3::Data::nle, se3::Model::nq, se3::Model::nv, se3::Data::sDUiJt, se3::cholesky::solve(), se3::Data::torque_residual, and se3::cholesky::Uiv().

Referenced by se3::python::AlgorithmsPythonVisitor::fd_llt_proxy().

void se3::forwardKinematics ( const Model &  model,
Data &  data,
const Eigen::VectorXd &  q 
) [inline]

Update the joint placement according to the current joint configuration.

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]qThe joint configuration (vector dim model.nq).

References se3::Model::joints, se3::Data::joints, se3::Model::nbody, se3::Model::nq, and se3::fusion::JointVisitor< ForwardKinematicZeroStep >::run().

Referenced by se3::python::AlgorithmsPythonVisitor::fk_0_proxy(), se3::python::AlgorithmsPythonVisitor::fk_1_proxy(), se3::python::AlgorithmsPythonVisitor::fk_2_proxy(), framesForwardKinematics(), kineticEnergy(), potentialEnergy(), and updateGeometryPlacements().

void se3::forwardKinematics ( const Model &  model,
Data &  data,
const Eigen::VectorXd &  q,
const Eigen::VectorXd &  v 
) [inline]

Update the joint placement according to the current joint configuration and velocity.

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]qThe joint configuration (vector dim model.nq).
[in]vThe joint velocity (vector dim model.nv).

References se3::Model::joints, se3::Data::joints, se3::Model::nbody, se3::Model::nq, se3::Model::nv, se3::fusion::JointVisitor< ForwardKinematicFirstStep >::run(), and se3::Data::v.

void se3::forwardKinematics ( const Model &  model,
Data &  data,
const Eigen::VectorXd &  q,
const Eigen::VectorXd &  v,
const Eigen::VectorXd &  a 
) [inline]

Update the joint placement according to the current joint configuration, velocity and acceleration.

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]qThe joint configuration (vector dim model.nq).
[in]vThe joint velocity (vector dim model.nv).
[in]aThe joint acceleration (vector dim model.nv).

References se3::Data::a, se3::Model::joints, se3::Data::joints, se3::Model::nbody, se3::Model::nq, se3::Model::nv, se3::fusion::JointVisitor< ForwardKinematicSecondStep >::run(), and se3::Data::v.

void se3::framesForwardKinematics ( const Model &  model,
Data &  data 
) [inline]

Update the position of each extra frame.

Parameters:
[in]modelThe kinematic model
dataData associated to model
Warning:
One of the algorithms forwardKinematics should have been called first

References se3::Model::nOperationalFrames, se3::Data::oMi, se3::Data::oMof, and se3::Model::operational_frames.

Referenced by se3::python::AlgorithmsPythonVisitor::frames_fk_0_proxy(), and framesForwardKinematics().

void se3::framesForwardKinematics ( const Model &  model,
Data &  data,
const Eigen::VectorXd &  q 
) [inline]

Compute Kinematics of full model, then the position of each operational frame.

Parameters:
[in]modelThe kinematic model
dataData associated to model
[in]qConfiguration vector

References forwardKinematics(), and framesForwardKinematics().

const SE3::Vector3& se3::getComFromCrba ( const Model &  model,
Data &  data 
) [inline]

Extracts the center of mass position from the joint space inertia matrix (also called the mass matrix).

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
Returns:
The center of mass position of the rigid body system expressed in the world frame (vector 3).
template<bool localFrame>
void se3::getFrameJacobian ( const Model &  model,
const Data &  data,
const Model::FrameIndex  frame_id,
Data::Matrix6x &  J 
) [inline]

Return the jacobian of the operational frame in the world frame or in the local frame depending on the template argument.

Parameters:
[in]modelThe kinematic model
[in]dataData associated to model
[in]frame_idId of the operational frame we want to compute the jacobian
JThe filled Jacobian Matrix
Template Parameters:
localFrameExpress the jacobian in the local or global frame
Warning:
The function computeJacobians should have been called first

References se3::SE3Base< Derived >::actInv(), idx_v(), se3::Data::J, se3::Model::joints, nv(), se3::Data::oMi, se3::Data::oMof, se3::Model::operational_frames, se3::Frame::placement, and se3::SE3Base< Derived >::translation().

template<bool localFrame>
void se3::getJacobian ( const Model &  model,
const Data &  data,
const Model::JointIndex  jointId,
Data::Matrix6x &  J 
)

Computes the Jacobian of a specific joint frame expressed either in the world frame or in the local frame of the joint.

Note:
This jacobian is extracted from data.J. You have to run se3::computeJacobians before calling it.
Parameters:
[in]localFrameExpressed the Jacobian in the local frame or world frame coordinates system.
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]jointIdThe id of the joint.
[out]JA reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill J with zero elements, e.g. J.fill(0.).
const Data::Matrix3x& se3::getJacobianComFromCrba ( const Model &  model,
Data &  data 
) [inline]

Extracts both the jacobian of the center of mass (CoM) and the CoM position from the joint space inertia matrix (also called the mass matrix).

The results are accessible through data.Jcom and data.com[0] and are both expressed in the world frame.

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
Returns:
The jacobian of the CoM expressed in the world frame (matrix 3 x model.nv).
JointIndex se3::id ( const JointModelVariant &  jmodel) [inline]

Visit a JointModelVariant through JointIdVisitor to get the index of the joint in the kinematic chain.

Parameters:
[in]jmodelThe JointModelVariant
Returns:
The index of the joint in the kinematic chain
int se3::idx_q ( const JointModelVariant &  jmodel) [inline]

Visit a JointModelVariant through JointIdxQVisitor to get the index in the full model configuration space corresponding to the first degree of freedom of the Joint.

Parameters:
[in]jmodelThe JointModelVariant
Returns:
The index in the full model configuration space corresponding to the first degree of freedom of jmodel
int se3::idx_v ( const JointModelVariant &  jmodel) [inline]

Visit a JointModelVariant through JointIdxVVisitor to get the index in the full model tangent space corresponding to the first joint tangent space degree.

Parameters:
[in]jmodelThe JointModelVariant
Returns:
The index in the full model tangent space corresponding to the first joint tangent space degree

Referenced by getFrameJacobian().

const Eigen::VectorXd& se3::impulseDynamics ( const Model &  model,
Data &  data,
const Eigen::VectorXd &  q,
const Eigen::VectorXd &  v_before,
const Eigen::MatrixXd &  J,
const double  r_coeff = 0.,
const bool  updateKinematics = true 
) [inline]

Compute the impulse dynamics with contact constraints.

Note:
It computes the following problem:
\( \begin{eqnarray} \underset{\dot{q}^{+}}{\min} & & \| \dot{q}^{+} - \dot{q}^{-} \|_{M(q)} \\ \text{s.t.} & & J (q) \dot{q}^{+} = - \epsilon J (q) \dot{q}^{-} \end{eqnarray} \)

where \( \dot{q}^{-} \) is the generalized velocity before impact, \( M \) is the joint space mass matrix, \( J \) the constraint Jacobian and \( \epsilon \) is the coefficient of restitution (1 for a fully elastic impact or 0 for a rigid impact).
Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]qThe joint configuration (vector dim model.nq).
[in]v_beforeThe joint velocity before impact (vector dim model.nv).
[in]JThe Jacobian of the constraints (dim nb_constraints*model.nv).
[in]r_coeffThe coefficient of restitution. Must be in [0;1].
[in]updateKinematicsIf true, the algorithm calls first se3::crba. Otherwise, it uses the current mass matrix value stored in data.
Returns:
A reference to the generalized velocity after impact stored in data.dq_after. The Lagrange Multipliers linked to the contact impulsed are available throw data.impulse_c vector.

References crba(), se3::Data::D, se3::cholesky::decompose(), se3::Data::dq_after, se3::Data::impulse_c, se3::Data::JMinvJt, se3::Data::llt_JMinvJt, se3::Model::nq, se3::Model::nv, se3::Data::sDUiJt, se3::cholesky::solve(), and se3::cholesky::Uiv().

Referenced by se3::python::AlgorithmsPythonVisitor::id_llt_proxy().

Eigen::VectorXd se3::integrate ( const Model &  model,
const Eigen::VectorXd &  q,
const Eigen::VectorXd &  v 
) [inline]

Integrate a configuration for the specified model for a tangent vector during one unit time.

Parameters:
[in]modelModel that must be integrated
[in]qInitial configuration (size model.nq)
[in]vVelocity (size model.nv)
Returns:
The integrated configuration (size model.nq)

References se3::Model::joints, se3::Model::nbody, se3::Model::nq, and se3::fusion::JointModelVisitor< IntegrateStep >::run().

Referenced by se3::python::AlgorithmsPythonVisitor::integrate_proxy().

Eigen::VectorXd se3::integrate ( const JointModelVariant &  jmodel,
const Eigen::VectorXd &  q,
const Eigen::VectorXd &  v 
) [inline]

Visit a JointModelVariant through JointIntegrateVisitor to integrate joint's configuration for a tangent vector during one unit time.

Parameters:
[in]jmodelThe JointModelVariant
[in]qInitatial configuration (size full model.nq)
[in]vJoints velocity (size full model.nv)
Returns:
The configuration integrated
Eigen::VectorXd se3::interpolate ( const Model &  model,
const Eigen::VectorXd &  q0,
const Eigen::VectorXd &  q1,
const double  u 
) [inline]

Interpolate the model between two configurations.

Parameters:
[in]modelModel to be interpolated
[in]q0Initial configuration vector (size model.nq)
[in]q1Final configuration vector (size model.nq)
[in]uu in [0;1] position along the interpolation.
Returns:
The interpolated configuration (q0 if u = 0, q1 if u = 1)

References se3::Model::joints, se3::Model::nbody, se3::Model::nq, and se3::fusion::JointModelVisitor< InterpolateStep >::run().

Referenced by se3::python::AlgorithmsPythonVisitor::interpolate_proxy().

Eigen::VectorXd se3::interpolate ( const JointModelVariant &  jmodel,
const Eigen::VectorXd &  q0,
const Eigen::VectorXd &  q1,
const double  u 
) [inline]

Visit a JointModelVariant through JointInterpolateVisitor to compute the interpolation between two joint's configurations.

Parameters:
[in]jmodelThe JointModelVariant
[in]q0Initial configuration to interpolate
[in]q1Final configuration to interpolate
[in]uu in [0;1] position along the interpolation.
Returns:
The interpolated configuration (q0 if u = 0, q1 if u = 1)
const Data::Matrix6x& se3::jacobian ( const Model &  model,
Data &  data,
const Eigen::VectorXd &  q,
const Model::JointIndex  jointId 
) [inline]

Computes the Jacobian of a specific joint frame expressed in the local frame of the joint.

The result is stored in data.J.

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]qThe joint configuration vector (dim model.nq).
[in]jointIdThe id of the joint.
Returns:
The Jacobian of the specific joint frame expressed in the local frame of the joint (matrix 6 x model.nv).
const Data::Matrix3x& se3::jacobianCenterOfMass ( const Model &  model,
Data &  data,
const Eigen::VectorXd &  q,
const bool  computeSubtreeComs = true 
) [inline]

Computes both the jacobian and the the center of mass position of a given model according to a particular joint configuration.

The results are accessible through data.Jcom and data.com[0] and are both expressed in the world frame. In addition, the algorithm also computes the Jacobian of all the joints (

See also:
se3::computeJacobians). And data.com[i] gives the center of mass of the subtree supported by joint i (expressed in the joint i frame).
Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]qThe joint configuration vector (dim model.nq).
[in]computeSubtreeComsIf true, the algorithm computes also the center of mass of the subtrees.
Returns:
The jacobian of center of mass position of the rigid body system expressed in the world frame (matrix 3 x model.nv).

Referenced by se3::python::AlgorithmsPythonVisitor::Jcom_proxy().

SE3 se3::joint_transform ( const JointDataVariant &  jdata) [inline]

Visit a JointDataVariant through JointTransformVisitor to get the joint internal transform (transform between the entry frame and the exit frame of the joint)

Parameters:
[in]jdataThe jdata
Returns:
The joint transform corresponding to the joint derived transform (sXp)

Referenced by se3::JointDataAccessor::M().

double se3::kineticEnergy ( const Model &  model,
Data &  data,
const Eigen::VectorXd &  q,
const Eigen::VectorXd &  v,
const bool  update_kinematics = true 
) [inline]

Computes the kinetic energy of the system.

The result is accessible through data.kinetic_energy.

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]qThe joint configuration vector (dim model.nq).
[in]vThe joint velocity vector (dim model.nv).
Returns:
The kinetic energy of the system in [J].

References forwardKinematics(), se3::Model::inertias, se3::Data::kinetic_energy, se3::Model::nbody, and se3::Data::v.

Referenced by computeAllTerms(), and se3::python::AlgorithmsPythonVisitor::kineticEnergy_proxy().

void se3::loadPolyhedronFromResource ( const std::string &  resource_path,
const ::urdf::Vector3 scale,
const Polyhedron_ptr &  polyhedron 
) throw (std::invalid_argument) [inline]

Read a mesh file and convert it to a polyhedral mesh.

Parameters:
[in]resource_pathPath to the ressource mesh file to be read
[in]scaleScale to apply when reading the ressource
[out]polyhedronThe resulted polyhedron

References meshFromAssimpScene().

Referenced by se3::urdf::retrieveCollisionGeometry().

template<typename D >
Eigen::Matrix<typename D::Scalar,3,1,Eigen::internal::traits<D>::Options> se3::log3 ( const Eigen::MatrixBase< D > &  R)

Log: SO3 -> so3.

Pseudo-inverse of log from SO3 -> { v so3, ||v|| < 2pi }.

Parameters:
[in]RThe rotation matrix.
Returns:
The angular velocity vector associated to the rotation matrix.

Referenced by se3::python::ExplogPythonVisitor::log3_proxy(), and log6().

template<typename _Scalar , int _Options>
MotionTpl<_Scalar,_Options> se3::log6 ( const SE3Tpl< _Scalar, _Options > &  M)

Log: SE3 -> se3.

Pseudo-inverse of exp from SE3 -> { v,w se3, ||w|| < 2pi }.

Parameters:
[in]MThe rigid transformation.
Returns:
The twist associated to the rigid transformation during time 1.

References alphaSkew(), log3(), se3::SE3Base< SE3Tpl< _Scalar, _Options > >::rotation(), SINCOS, and se3::SE3Base< SE3Tpl< _Scalar, _Options > >::translation().

Referenced by log6(), se3::python::ExplogPythonVisitor::log6FromMatrix_proxy(), and se3::python::ExplogPythonVisitor::log6FromSE3_proxy().

template<typename D >
MotionTpl<typename D::Scalar,Eigen::internal::traits<D>::Options> se3::log6 ( const Eigen::MatrixBase< D > &  M)

Log: SE3 -> se3.

Pseudo-inverse of exp from SE3 -> { v,w se3, ||w|| < 2pi }.

Parameters:
[in]RThe rigid transformation represented as an homogenous matrix.
Returns:
The twist associated to the rigid transformation during time 1.

References log6().

void se3::meshFromAssimpScene ( const std::string &  name,
const ::urdf::Vector3 scale,
const aiScene *  scene,
const Polyhedron_ptr &  mesh 
) throw (std::invalid_argument) [inline]

Convert an assimp scene to a mesh.

Parameters:
[in]nameFile (ressource) transformed into an assimp scene in loa
[in]scaleScale to apply when reading the ressource
[in]scenePointer to the assimp scene
[out]meshThe mesh that must be built

References buildMesh(), se3::TriangleAndVertices::clear(), se3::TriangleAndVertices::triangles_, and se3::TriangleAndVertices::vertices_.

Referenced by loadPolyhedronFromResource().

Motion se3::motion ( const JointDataVariant &  jdata) [inline]

Visit a JointDataVariant through JointMotionVisitor to get the joint internal motion as a dense motion.

Parameters:
[in]jdataThe jdata
Returns:
The motion dense corresponding to the joint derived motion

Referenced by se3::JointDataAccessor::v().

const Eigen::VectorXd& se3::nonLinearEffects ( const Model &  model,
Data &  data,
const Eigen::VectorXd &  q,
const Eigen::VectorXd &  v 
) [inline]

Computes the non-linear effects (Corriolis, centrifual and gravitationnal effects), also called the biais terms \( b(q,\dot{q}) \) of the Lagrangian dynamics:

\( \begin{eqnarray} M \ddot{q} + b(q, \dot{q}) = \tau \end{eqnarray} \)


Note:
This function is equivalent to se3::rnea(model, data, q, v, 0).
Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]qThe joint configuration vector (dim model.nq).
[in]vThe joint velocity vector (dim model.nv).
Returns:
The biais terms stored in data.nle.

Referenced by se3::python::AlgorithmsPythonVisitor::nle_proxy().

int se3::nq ( const JointModelVariant &  jmodel) [inline]

Visit a JointModelVariant through JointNqVisitor to get the dimension of the joint configuration space.

Parameters:
[in]jmodelThe JointModelVariant
Returns:
The dimension of joint configuration space
int se3::nv ( const JointModelVariant &  jmodel) [inline]

Visit a JointModelVariant through JointNvVisitor to get the dimension of the joint tangent space.

Parameters:
[in]jmodelThe JointModelVariant
Returns:
The dimension of joint tangent space

Referenced by exp3(), and getFrameJacobian().

template<typename D >
Motion se3::operator* ( const ConstraintIdentity &  ,
const Eigen::MatrixBase< D > &  v 
)
Inertia::Matrix6 se3::operator* ( const Inertia &  Y,
const ConstraintIdentity &   
) [inline]
const Inertia::Matrix6& se3::operator* ( const Inertia::Matrix6 &  Y,
const ConstraintIdentity &   
) [inline]
Inertia::Matrix6& se3::operator* ( Inertia::Matrix6 &  Y,
const ConstraintIdentity &   
) [inline]
Inertia::Matrix6 se3::operator* ( const ConstraintIdentity::TransposeConst &  ,
const Inertia &  Y 
) [inline]
template<typename D >
const Eigen::MatrixBase<D>& se3::operator* ( const ConstraintIdentity::TransposeConst &  ,
const Eigen::MatrixBase< D > &  F 
)
template<typename D >
Motion se3::operator* ( const ConstraintRotationalSubspace &  ,
const Eigen::MatrixBase< D > &  v 
)
template<typename D >
Motion se3::operator* ( const ConstraintTranslationSubspace &  ,
const Eigen::MatrixBase< D > &  v 
)
Eigen::Matrix<double, 6, 3> se3::operator* ( const Inertia &  Y,
const ConstraintRotationalSubspace &   
) [inline]
Eigen::Matrix<double, 6, 3> se3::operator* ( const Inertia &  Y,
const ConstraintTranslationSubspace &   
) [inline]
template<typename D >
Motion se3::operator* ( const ConstraintPlanar &  ,
const Eigen::MatrixBase< D > &  v 
)
Eigen::Matrix<double,6,1> se3::operator* ( const Inertia &  Y,
const ConstraintPrismaticUnaligned &  cpu 
) [inline]
Eigen::Matrix<double,6,1> se3::operator* ( const Inertia &  Y,
const ConstraintRevoluteUnaligned &  cru 
) [inline]
template<typename _Scalar , int _Options>
Eigen::Matrix<_Scalar,6,3,_Options> se3::operator* ( const InertiaTpl< _Scalar, _Options > &  Y,
const typename JointSphericalZYXTpl< _Scalar, _Options >::ConstraintRotationalSubspace &  S 
)
Eigen::ProductReturnType< Eigen::Block<const Inertia::Matrix6,6,3>, const ConstraintPrismaticUnaligned::Vector3 >::Type se3::operator* ( const Inertia::Matrix6 &  Y,
const ConstraintPrismaticUnaligned &  cpu 
) [inline]
Eigen::ProductReturnType< Eigen::Block<const Inertia::Matrix6,6,3>, const ConstraintRevoluteUnaligned::Vector3 >::Type se3::operator* ( const Inertia::Matrix6 &  Y,
const ConstraintRevoluteUnaligned &  cru 
) [inline]
template<typename _Scalar , int _Options>
Eigen::ProductReturnType< const Eigen::Block<const Inertia::Matrix6,6,3>, const typename JointSphericalZYXTpl<_Scalar,_Options>::ConstraintRotationalSubspace::Matrix3 >::Type se3::operator* ( const typename InertiaTpl< _Scalar, _Options >::Matrix6 &  Y,
const typename JointSphericalZYXTpl< _Scalar, _Options >::ConstraintRotationalSubspace &  S 
)
Eigen::Matrix<Inertia::Scalar_t, 6, 3> se3::operator* ( const Inertia::Matrix6 &  Y,
const ConstraintPlanar &   
) [inline]
Eigen::Matrix<double,6,1> se3::operator* ( const Inertia &  Y,
const ConstraintPrismatic< 0 > &   
) [inline]
Eigen::Matrix<double,6,1> se3::operator* ( const Inertia &  Y,
const ConstraintPrismatic< 1 > &   
) [inline]
Eigen::Matrix<double,6,1> se3::operator* ( const Inertia &  Y,
const ConstraintRevolute< 0 > &   
) [inline]
Eigen::Matrix<double,6,1> se3::operator* ( const Inertia &  Y,
const ConstraintPrismatic< 2 > &   
) [inline]
Eigen::Matrix<double,6,1> se3::operator* ( const Inertia &  Y,
const ConstraintRevolute< 1 > &   
) [inline]
template<int axis>
const Eigen::MatrixBase<const Inertia::Matrix6>::ColXpr se3::operator* ( const Inertia::Matrix6 &  Y,
const ConstraintPrismatic< axis > &   
) [inline]
Eigen::Matrix<double,6,1> se3::operator* ( const Inertia &  Y,
const ConstraintRevolute< 2 > &   
) [inline]
template<int axis>
const Eigen::MatrixBase<const Inertia::Matrix6>::ColXpr se3::operator* ( const Inertia::Matrix6 &  Y,
const ConstraintRevolute< axis > &   
) [inline]
template<int axis>
Eigen::MatrixBase<Inertia::Matrix6>::ColXpr se3::operator* ( Inertia::Matrix6 &  Y,
const ConstraintRevolute< axis > &   
) [inline]
const MotionPrismaticUnaligned& se3::operator+ ( const MotionPrismaticUnaligned &  m,
const BiasZero &   
) [inline]
Motion se3::operator+ ( const MotionPrismaticUnaligned &  m1,
const Motion &  m2 
) [inline]
const MotionRevoluteUnaligned& se3::operator+ ( const MotionRevoluteUnaligned &  m,
const BiasZero &   
) [inline]
const MotionPlanar se3::operator+ ( const MotionPlanar &  m,
const BiasZero &   
) [inline]
Motion se3::operator+ ( const MotionRevoluteUnaligned &  m1,
const Motion &  m2 
) [inline]
const MotionSpherical se3::operator+ ( const MotionSpherical &  m,
const BiasZero &   
) [inline]
Motion se3::operator+ ( const MotionSpherical &  m1,
const Motion &  m2 
) [inline]
const MotionTranslation se3::operator+ ( const MotionTranslation &  m,
const BiasZero &   
) [inline]
Motion se3::operator+ ( const MotionTranslation &  m1,
const Motion &  m2 
) [inline]
template<int axis>
const MotionPrismatic<axis>& se3::operator+ ( const MotionPrismatic< axis > &  m,
const BiasZero &   
)
template<int axis>
const MotionRevolute<axis>& se3::operator+ ( const MotionRevolute< axis > &  m,
const BiasZero &   
)
template<int axis>
Motion se3::operator+ ( const MotionPrismatic< axis > &  m1,
const Motion &  m2 
)
template<int axis>
Motion se3::operator+ ( const MotionRevolute< axis > &  m1,
const Motion &  m2 
)
const Motion& se3::operator+ ( const Motion &  v,
const BiasZero &   
) [inline]
const Motion& se3::operator+ ( const BiasZero &  ,
const Motion &  v 
) [inline]
std::ostream& se3::operator<< ( std::ostream &  os,
const GeometryObject &  geom_object 
) [inline]
bool se3::operator== ( const fcl::CollisionObject &  lhs,
const fcl::CollisionObject &  rhs 
)

Return true if the intrinsic geometry of the two CollisionObject is the same.

bool se3::operator== ( const GeometryObject &  lhs,
const GeometryObject &  rhs 
) [inline]
Motion se3::operator^ ( const Motion &  m1,
const MotionSpherical &  m2 
) [inline]
Motion se3::operator^ ( const Motion &  m1,
const MotionTranslation &  m2 
) [inline]
Motion se3::operator^ ( const Motion &  m1,
const MotionPrismaticUnaligned &  m2 
) [inline]
Motion se3::operator^ ( const Motion &  m1,
const JointSphericalZYX::MotionSpherical &  m2 
) [inline]
Motion se3::operator^ ( const Motion &  m1,
const MotionRevoluteUnaligned &  m2 
) [inline]
Motion se3::operator^ ( const Motion &  m1,
const MotionRevolute< 0 > &  m2 
) [inline]
Motion se3::operator^ ( const Motion &  m1,
const MotionPrismatic< 0 > &  m2 
) [inline]
Motion se3::operator^ ( const Motion &  m1,
const MotionRevolute< 1 > &  m2 
) [inline]
Motion se3::operator^ ( const Motion &  m1,
const MotionPrismatic< 1 > &  m2 
) [inline]
Motion se3::operator^ ( const Motion &  m1,
const MotionRevolute< 2 > &  m2 
) [inline]
Motion se3::operator^ ( const Motion &  m1,
const MotionPrismatic< 2 > &  m2 
) [inline]
template<typename S , int O>
MotionTpl<S,O> se3::operator^ ( const MotionTpl< S, O > &  m1,
const MotionTpl< S, O > &  m2 
)
template<typename S , int O>
ForceTpl<S,O> se3::operator^ ( const MotionTpl< S, O > &  m,
const ForceTpl< S, O > &  f 
)
double se3::potentialEnergy ( const Model &  model,
Data &  data,
const Eigen::VectorXd &  q,
const bool  update_kinematics = true 
) [inline]

Computes the potential energy of the system, i.e.

the potential energy linked to the gravity field. The result is accessible through data.potential_energy.

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]qThe joint configuration vector (dim model.nq).
Returns:
The potential energy of the system in [J].

References forwardKinematics(), se3::Model::gravity, se3::Model::inertias, se3::MotionBase< Derived >::linear(), se3::Model::nbody, se3::Data::oMi, and se3::Data::potential_energy.

Referenced by computeAllTerms(), and se3::python::AlgorithmsPythonVisitor::potentialEnergy_proxy().

std::string se3::random ( const int  len) [inline]
Eigen::VectorXd se3::randomConfiguration ( const Model &  model,
const Eigen::VectorXd &  lowerLimits,
const Eigen::VectorXd &  upperLimits 
) [inline]

Generate a configuration vector uniformly sampled among provided limits.

Warning:
If limits are infinite, exceptions may be thrown in the joint implementation of uniformlySample
Parameters:
[in]modelModel we want to generate a configuration vector of
[in]lowerLimitsJoints lower limits
[in]upperLimitsJoints upper limits
Returns:
The resulted configuration vector (size model.nq)

References se3::Model::joints, se3::Model::nbody, se3::Model::nq, and se3::fusion::JointModelVisitor< RandomConfiguration >::run().

Referenced by randomConfiguration(), and se3::python::AlgorithmsPythonVisitor::randomConfiguration_proxy().

Eigen::VectorXd se3::randomConfiguration ( const Model &  model) [inline]

Generate a configuration vector uniformly sampled among the joint limits of the specified Model.

Warning:
If limits are infinite (no one specified when adding a body or no modification directly in my_model.{lowerPositionLimit,upperPositionLimit}, exceptions may be thrown in the joint implementation of uniformlySample
Parameters:
[in]modelModel we want to generate a configuration vector of
Returns:
The resulted configuration vector (size model.nq)

References se3::Model::lowerPositionLimit, randomConfiguration(), and se3::Model::upperPositionLimit.

Eigen::VectorXd se3::randomConfiguration ( const JointModelVariant &  jmodel,
const Eigen::VectorXd &  lower_pos_limit,
const Eigen::VectorXd &  upper_pos_limit 
) [inline]

Visit a JointModelVariant through JointRandomConfigurationVisitor to generate a configuration vector uniformly sampled among provided limits.

Parameters:
[in]jmodelThe JointModelVariant
[in]lower_pos_limitlower joint limit
[in]upper_pos_limitupper joint limit
Returns:
The joint configuration
const Eigen::VectorXd& se3::rnea ( const Model &  model,
Data &  data,
const Eigen::VectorXd &  q,
const Eigen::VectorXd &  v,
const Eigen::VectorXd &  a 
) [inline]

The Recursive Newton-Euler algorithm.

It computes the inverse dynamics, aka the joint torques according to the current state of the system and the desired joint accelerations.

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]qThe joint configuration vector (dim model.nq).
[in]vThe joint velocity vector (dim model.nv).
[in]aThe joint acceleration vector (dim model.nv).
Returns:
The desired joint torques stored in data.tau.

Referenced by se3::python::AlgorithmsPythonVisitor::rnea_proxy().

void se3::setIndexes ( JointModelVariant &  jmodel,
JointIndex  id,
int  q,
int  v 
) [inline]

Visit a JointModelVariant through JointSetIndexesVisitor to set the indexes of the joint in the kinematic chain.

Parameters:
[in]jmodelThe JointModelVariant
[in]idThe index of joint in the kinematic chain
[in]qThe index in the full model configuration space corresponding to the first degree of freedom
[in]vThe index in the full model tangent space corresponding to the first joint tangent space degree
Returns:
The index of the joint in the kinematic chain
template<typename D >
Eigen::Matrix<typename D::Scalar,3,3,D::Options> se3::skew ( const Eigen::MatrixBase< D > &  v) [inline]
fcl::Matrix3f se3::toFclMatrix3f ( const Eigen::Matrix3d &  mat) [inline]

Referenced by toFclTransform3f().

fcl::Transform3f se3::toFclTransform3f ( const se3::SE3 m) [inline]
fcl::Vec3f se3::toFclVec3f ( const Eigen::Vector3d &  vec) [inline]

Referenced by toFclTransform3f().

Eigen::Matrix3d se3::toMatrix3d ( const fcl::Matrix3f &  mat) [inline]

Referenced by toPinocchioSE3().

se3::SE3 se3::toPinocchioSE3 ( const fcl::Transform3f &  tf) [inline]

References toMatrix3d(), and toVector3d().

Eigen::Vector3d se3::toVector3d ( const fcl::Vec3f &  vec) [inline]
Eigen::Matrix<double,6,Eigen::Dynamic> se3::u_inertia ( const JointDataVariant &  jdata) [inline]

Visit a JointDataVariant through JointUInertiaVisitor to get the U matrix of the inertia matrix decomposition.

Parameters:
[in]jdataThe jdata
Returns:
The U matrix of the inertia matrix decomposition

Referenced by se3::JointDataAccessor::U().

Eigen::Matrix<double,6,Eigen::Dynamic> se3::udinv_inertia ( const JointDataVariant &  jdata) [inline]

Visit a JointDataVariant through JointUDInvInertiaVisitor to get U*D^{-1} matrix of the inertia matrix decomposition.

Parameters:
[in]jdataThe jdata
Returns:
The U*D^{-1} matrix of the inertia matrix decomposition

Referenced by se3::JointDataAccessor::UDinv().

template<typename D >
Eigen::Matrix<typename D::Scalar,3,1,D::Options> se3::unSkew ( const Eigen::MatrixBase< D > &  M) [inline]

Inverse operation related to skew.

From a given skew-symmetric matrix M of dimension 3x3, it extracts the supporting vector, i.e. the entries of M.

Parameters:
[in]MA 3x3 matrix.
Returns:
The vector entries of the skew-symmetric matrix.

Referenced by se3::InertiaTpl< double, 0 >::InertiaTpl().

void se3::updateGeometryPlacements ( const Model &  model,
Data &  data,
const GeometryModel &  geom,
GeometryData &  geom_data,
const Eigen::VectorXd &  q 
) [inline]

Apply a forward kinematics and update the placement of the geometry objects (both collision's and visual's one).

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]geomThe geometry model containing the collision objects.
[out]geom_dataThe geometry data containing the placements of the collision objects. See oMg field in GeometryData.
[in]qThe joint configuration vector (dim model.nq).

References forwardKinematics().

Referenced by computeCollisions(), and computeDistances().

void se3::updateGeometryPlacements ( const Model &  model,
const Data &  data,
const GeometryModel &  geom,
GeometryData &  geom_data 
) [inline]

Update the placement of the geometry objects according to the current joint placements contained in data.

(both collision's and visual's one)

Parameters:
[in]modelThe model structure of the rigid body system.
[in]dataThe data structure of the rigid body system.
[in]geomThe geometry model containing the collision objects.
[out]geom_dataThe geometry data containing the placements of the collision objects. See oMg field in GeometryData.

References se3::GeometryModel::collision_objects, se3::GeometryData::model_geom, se3::GeometryModel::ncollisions, se3::GeometryModel::nvisuals, se3::GeometryData::oMg_collisions, se3::GeometryData::oMg_fcl_collisions, se3::GeometryData::oMg_visuals, se3::Data::oMi, toFclTransform3f(), and se3::GeometryModel::visual_objects.


Variable Documentation