mc_rbdyn Namespace Reference

Namespaces

 detail
 
 details
 
 gui
 
 lipm_stabilizer
 

Classes

struct  Base
 
struct  BodySensor
 
struct  Collision
 
struct  CompoundJointConstraintDescription
 
struct  Contact
 
struct  CylindricalSurface
 
struct  Device
 
struct  DevicePtrVector
 
struct  Flexibility
 
struct  ForceSensor
 
struct  Frame
 
struct  Gains
 
struct  GripperSurface
 
struct  JointSensor
 
struct  LoadRobotParameters
 
struct  Mimic
 Stores mimic joint information. More...
 
struct  PlanarSurface
 
struct  Plane
 
struct  PolygonInterpolator
 
struct  QuadraticGenerator
 
struct  Robot
 
struct  RobotConverter
 Copies all common properties from one robot to another encoders, sensors, etc) More...
 
struct  RobotConverterConfig
 Configuration for mc_rbdyn::RobotConverter. More...
 
struct  RobotData
 
struct  RobotFrame
 
class  RobotLoader
 Load RobotModule instances from shared libraries. More...
 
struct  RobotModule
 
struct  Robots
 
struct  Springs
 
struct  Surface
 
struct  VisualMap
 

Typedefs

typedef std::vector< BodySensor, Eigen::aligned_allocator< BodySensor > > BodySensorVector
 
using CompoundJointConstraintDescriptionVector = std::vector< CompoundJointConstraintDescription, Eigen::aligned_allocator< CompoundJointConstraintDescription > >
 
using DevicePtr = std::unique_ptr< Device >
 
using Sensor = Device
 
using SensorPtr = DevicePtr
 
using RobotsPtr = std::shared_ptr< Robots >
 
using RobotPtr = std::shared_ptr< Robot >
 
using ConstRobotPtr = std::shared_ptr< const Robot >
 
using RobotFramePtr = std::shared_ptr< RobotFrame >
 
using ConstRobotFramePtr = std::shared_ptr< const RobotFrame >
 
using FramePtr = std::shared_ptr< Frame >
 
using ConstFramePtr = std::shared_ptr< const Frame >
 
using Gains2d = Gains< 2 >
 
using Gains3d = Gains< 3 >
 
using Gains6d = Gains< 6 >
 
using RobotDataPtr = std::shared_ptr< RobotData >
 
using S_ObjectPtr = std::shared_ptr< sch::S_Object >
 
typedef std::shared_ptr< RobotModuleRobotModulePtr
 
using RobotModuleVector = std::vector< RobotModule, Eigen::aligned_allocator< RobotModule > >
 
typedef std::shared_ptr< SurfaceSurfacePtr
 

Functions

bool operator== (const mc_rbdyn::BodySensor &lhs, const mc_rbdyn::BodySensor &rhs)
 
MC_RBDYN_DLLAPI std::ostream & operator<< (std::ostream &os, const Collision &c)
 
MC_RBDYN_DLLAPI std::vector< sva::PTransformd > computePoints (const mc_rbdyn::Surface &robotSurface, const mc_rbdyn::Surface &envSurface, const sva::PTransformd &X_es_rs)
 
MC_RBDYN_DLLAPI sva::PTransformd planar (const double &T, const double &B, const double &N_rot)
 
MC_RBDYN_DLLAPI sva::PTransformd cylindrical (const double &T, const double &T_rot)
 
MC_RBDYN_DLLAPI void planarParam (const sva::PTransformd &X_es_rs, double &T, double &B, double &N_rot)
 
MC_RBDYN_DLLAPI void cylindricalParam (const sva::PTransformd &X_es_rs, double &T, double &T_rot)
 
MC_RBDYN_DLLAPI std::vector< double > jointParam (const Surface &r1Surface, const Surface &r2Surface, const sva::PTransformd &X_es_rs)
 
bool operator== (const mc_rbdyn::ForceSensor &lhs, const mc_rbdyn::ForceSensor &rhs)
 
Eigen::Matrix3d hat (const Eigen::Vector3d &v)
 
bool operator== (const JointSensor &lhs, const JointSensor &rhs)
 
bool operator== (const Mimic &lhs, const Mimic &rhs)
 
MC_RBDYN_DLLAPI std::vector< Planeplanes_from_polygon (const std::shared_ptr< geos::geom::Geometry > &geometry)
 
MC_RBDYN_DLLAPI std::vector< Eigen::Vector3d > points_from_polygon (std::shared_ptr< geos::geom::Geometry > geometry)
 
const MC_RBDYN_DLLAPI mc_rbdyn::RobotrobotFromConfig (const mc_rtc::Configuration &config, const mc_rbdyn::Robots &robots, const std::string &prefix, bool required=false, const std::string &robotIndexKey="robotIndex", const std::string &robotNameKey="robot", const std::string &defaultRobotName="")
 Obtains a reference to a loaded robot from configuration (using robotName or robotIndex) More...
 
std::string MC_RBDYN_DLLAPI robotNameFromConfig (const mc_rtc::Configuration &config, const mc_rbdyn::Robots &robots, const std::string &prefix="", bool required=false, const std::string &robotIndexKey="robotIndex", const std::string &robotNameKey="robot", const std::string &defaultRobotName="")
 Helper to obtain the robot name from configuration. More...
 
unsigned int MC_RBDYN_DLLAPI robotIndexFromConfig (const mc_rtc::Configuration &config, const mc_rbdyn::Robots &robots, const std::string &prefix="", bool required=false, const std::string &robotIndexKey="robotIndex", const std::string &robotNameKey="robot", const std::string &defaultRobotName="")
 Helper to obtain the robot index from configuration. More...
 
MC_RBDYN_DLLAPI RobotModule::bounds_t urdf_limits_to_bounds (const rbd::parsers::Limits &limits)
 Converts limits provided by RBDyn parsers to bounds. More...
 
MC_RBDYN_DLLAPI bool check_module_compatibility (const RobotModule &lhs, const RobotModule &rhs)
 
bool operator== (const RobotModule::Gripper::Safety &lhs, const RobotModule::Gripper::Safety &rhs)
 
bool operator== (const RobotModule::Gripper &lhs, const RobotModule::Gripper &rhs)
 
MC_RBDYN_DLLAPI RobotsPtr loadRobot (const RobotModule &module, const LoadRobotParameters &params={})
 
MC_RBDYN_DLLAPI RobotsPtr loadRobot (const std::string &name, const RobotModule &module, const LoadRobotParameters &params={})
 
MC_RBDYN_DLLAPI RobotsPtr loadRobots (const std::vector< std::shared_ptr< RobotModule >> &modules)
 
MC_RBDYN_DLLAPI RobotsPtr loadRobotAndEnv (const RobotModule &module, const RobotModule &envModule)
 
MC_RBDYN_DLLAPI RobotsPtr loadRobotFromUrdf (const std::string &name, const std::string &urdf, const rbd::parsers::ParserParameters &parser_params={}, const LoadRobotParameters &load_params={})
 
Eigen::Matrix3d rpyToMat (const double &r, const double &p, const double &y)
 
Eigen::Matrix3d rpyToMat (const Eigen::Vector3d &rpy)
 
sva::PTransformd rpyToPT (const Eigen::Vector3d &rpy)
 
sva::PTransformd rpyToPT (const double &r, const double &p, const double &y)
 
Eigen::Vector3d rpyFromMat (const Eigen::Matrix3d &E)
 
Eigen::Vector3d rpyFromQuat (const Eigen::Quaterniond &quat)
 
template<typename T >
Eigen::Vector3d rpyFromRotation (const T &value)
 
MC_RBDYN_DLLAPI sch::S_Object * surface_to_sch (const mc_rbdyn::Surface &surface, const double &depth=0.01, const unsigned int &slice=8)
 
MC_RBDYN_DLLAPI sch::S_Object * sch_polyhedron (const std::vector< sva::PTransformd > &points)
 
MC_RBDYN_DLLAPI sch::S_Object * planar_hull (const mc_rbdyn::PlanarSurface &surface, const double &depth)
 
MC_RBDYN_DLLAPI sch::S_Object * cylindrical_hull (const mc_rbdyn::CylindricalSurface &surface, const unsigned int &slice)
 
MC_RBDYN_DLLAPI sch::S_Object * gripper_hull (const mc_rbdyn::GripperSurface &surface, const double &depth)
 
MC_RBDYN_DLLAPI std::vector< std::shared_ptr< Surface > > readRSDFFromDir (const std::string &dirname)
 
Eigen::Vector3d MC_RBDYN_DLLAPI zmp (const sva::ForceVecd &netTotalWrench, const Eigen::Vector3d &plane_p, const Eigen::Vector3d &plane_n, double minimalNetNormalForce=1.)
 Actual ZMP computation from net total wrench and the ZMP plane. More...
 
bool MC_RBDYN_DLLAPI zmp (Eigen::Vector3d &zmpOut, const sva::ForceVecd &netTotalWrench, const Eigen::Vector3d &plane_p, const Eigen::Vector3d &plane_n, double minimalNetNormalForce=1.) noexcept
 Actual ZMP computation from net total wrench and the ZMP plane. More...
 
Eigen::Vector3d MC_RBDYN_DLLAPI zmp (const sva::ForceVecd &netTotalWrench, const sva::PTransformd &zmpFrame, double minimalNetNormalForce=1.)
 ZMP computation from net total wrench and a frame. More...
 
bool MC_RBDYN_DLLAPI zmp (Eigen::Vector3d &zmpOut, const sva::ForceVecd &netTotalWrench, const sva::PTransformd &zmpFrame, double minimalNetNormalForce=1.) noexcept
 ZMP computation from net total wrench and a frame. More...
 

Detailed Description

Forward declarations for the mc_rbdyn library

Interface used to load robots

Typedef Documentation

◆ BodySensorVector

typedef std::vector<BodySensor, Eigen::aligned_allocator<BodySensor> > mc_rbdyn::BodySensorVector

◆ CompoundJointConstraintDescriptionVector

◆ ConstFramePtr

using mc_rbdyn::ConstFramePtr = typedef std::shared_ptr<const Frame>

◆ ConstRobotFramePtr

using mc_rbdyn::ConstRobotFramePtr = typedef std::shared_ptr<const RobotFrame>

◆ ConstRobotPtr

using mc_rbdyn::ConstRobotPtr = typedef std::shared_ptr<const Robot>

◆ DevicePtr

using mc_rbdyn::DevicePtr = typedef std::unique_ptr<Device>

◆ FramePtr

using mc_rbdyn::FramePtr = typedef std::shared_ptr<Frame>

◆ Gains2d

using mc_rbdyn::Gains2d = typedef Gains<2>

◆ Gains3d

using mc_rbdyn::Gains3d = typedef Gains<3>

◆ Gains6d

using mc_rbdyn::Gains6d = typedef Gains<6>

◆ RobotDataPtr

using mc_rbdyn::RobotDataPtr = typedef std::shared_ptr<RobotData>

◆ RobotFramePtr

using mc_rbdyn::RobotFramePtr = typedef std::shared_ptr<RobotFrame>

◆ RobotModulePtr

typedef std::shared_ptr<RobotModule> mc_rbdyn::RobotModulePtr

◆ RobotModuleVector

using mc_rbdyn::RobotModuleVector = typedef std::vector<RobotModule, Eigen::aligned_allocator<RobotModule> >

◆ RobotPtr

using mc_rbdyn::RobotPtr = typedef std::shared_ptr<Robot>

◆ RobotsPtr

using mc_rbdyn::RobotsPtr = typedef std::shared_ptr<Robots>

◆ S_ObjectPtr

using mc_rbdyn::S_ObjectPtr = typedef std::shared_ptr<sch::S_Object>

◆ Sensor

using mc_rbdyn::Sensor = typedef Device

◆ SensorPtr

using mc_rbdyn::SensorPtr = typedef DevicePtr

◆ SurfacePtr

typedef std::shared_ptr<Surface> mc_rbdyn::SurfacePtr

Function Documentation

◆ check_module_compatibility()

MC_RBDYN_DLLAPI bool mc_rbdyn::check_module_compatibility ( const RobotModule lhs,
const RobotModule rhs 
)

Checks that two RobotModule are compatible for control

The requirements are:

  • Same reference joint order
  • Same force sensors
  • Same body sensors
  • Same joint sensors
  • Same grippers
  • Same devices
Returns
True if the two modules are compatible, false otherwise

◆ computePoints()

MC_RBDYN_DLLAPI std::vector<sva::PTransformd> mc_rbdyn::computePoints ( const mc_rbdyn::Surface robotSurface,
const mc_rbdyn::Surface envSurface,
const sva::PTransformd &  X_es_rs 
)

◆ cylindrical()

MC_RBDYN_DLLAPI sva::PTransformd mc_rbdyn::cylindrical ( const double &  T,
const double &  T_rot 
)

◆ cylindrical_hull()

MC_RBDYN_DLLAPI sch::S_Object* mc_rbdyn::cylindrical_hull ( const mc_rbdyn::CylindricalSurface surface,
const unsigned int &  slice 
)

◆ cylindricalParam()

MC_RBDYN_DLLAPI void mc_rbdyn::cylindricalParam ( const sva::PTransformd &  X_es_rs,
double &  T,
double &  T_rot 
)

◆ gripper_hull()

MC_RBDYN_DLLAPI sch::S_Object* mc_rbdyn::gripper_hull ( const mc_rbdyn::GripperSurface surface,
const double &  depth 
)

◆ hat()

Eigen::Matrix3d mc_rbdyn::hat ( const Eigen::Vector3d &  v)
inline

◆ jointParam()

MC_RBDYN_DLLAPI std::vector<double> mc_rbdyn::jointParam ( const Surface r1Surface,
const Surface r2Surface,
const sva::PTransformd &  X_es_rs 
)

◆ loadRobot() [1/2]

MC_RBDYN_DLLAPI RobotsPtr mc_rbdyn::loadRobot ( const RobotModule module,
const LoadRobotParameters params = {} 
)

◆ loadRobot() [2/2]

MC_RBDYN_DLLAPI RobotsPtr mc_rbdyn::loadRobot ( const std::string &  name,
const RobotModule module,
const LoadRobotParameters params = {} 
)

◆ loadRobotAndEnv()

MC_RBDYN_DLLAPI RobotsPtr mc_rbdyn::loadRobotAndEnv ( const RobotModule module,
const RobotModule envModule 
)

◆ loadRobotFromUrdf()

MC_RBDYN_DLLAPI RobotsPtr mc_rbdyn::loadRobotFromUrdf ( const std::string &  name,
const std::string &  urdf,
const rbd::parsers::ParserParameters &  parser_params = {},
const LoadRobotParameters load_params = {} 
)

◆ loadRobots()

MC_RBDYN_DLLAPI RobotsPtr mc_rbdyn::loadRobots ( const std::vector< std::shared_ptr< RobotModule >> &  modules)

◆ operator<<()

MC_RBDYN_DLLAPI std::ostream& mc_rbdyn::operator<< ( std::ostream &  os,
const Collision c 
)

◆ operator==() [1/6]

bool mc_rbdyn::operator== ( const JointSensor lhs,
const JointSensor rhs 
)
inline

◆ operator==() [2/6]

bool mc_rbdyn::operator== ( const mc_rbdyn::BodySensor lhs,
const mc_rbdyn::BodySensor rhs 
)
inline

◆ operator==() [3/6]

bool mc_rbdyn::operator== ( const mc_rbdyn::ForceSensor lhs,
const mc_rbdyn::ForceSensor rhs 
)
inline

◆ operator==() [4/6]

bool mc_rbdyn::operator== ( const Mimic lhs,
const Mimic rhs 
)
inline

◆ operator==() [5/6]

bool mc_rbdyn::operator== ( const RobotModule::Gripper lhs,
const RobotModule::Gripper rhs 
)
inline

◆ operator==() [6/6]

bool mc_rbdyn::operator== ( const RobotModule::Gripper::Safety lhs,
const RobotModule::Gripper::Safety rhs 
)
inline

◆ planar()

MC_RBDYN_DLLAPI sva::PTransformd mc_rbdyn::planar ( const double &  T,
const double &  B,
const double &  N_rot 
)

◆ planar_hull()

MC_RBDYN_DLLAPI sch::S_Object* mc_rbdyn::planar_hull ( const mc_rbdyn::PlanarSurface surface,
const double &  depth 
)

◆ planarParam()

MC_RBDYN_DLLAPI void mc_rbdyn::planarParam ( const sva::PTransformd &  X_es_rs,
double &  T,
double &  B,
double &  N_rot 
)

◆ planes_from_polygon()

MC_RBDYN_DLLAPI std::vector<Plane> mc_rbdyn::planes_from_polygon ( const std::shared_ptr< geos::geom::Geometry > &  geometry)

◆ points_from_polygon()

MC_RBDYN_DLLAPI std::vector<Eigen::Vector3d> mc_rbdyn::points_from_polygon ( std::shared_ptr< geos::geom::Geometry >  geometry)

◆ readRSDFFromDir()

MC_RBDYN_DLLAPI std::vector<std::shared_ptr<Surface> > mc_rbdyn::readRSDFFromDir ( const std::string &  dirname)

◆ rpyFromMat()

Eigen::Vector3d mc_rbdyn::rpyFromMat ( const Eigen::Matrix3d &  E)
inline

Roll-pitch-yaw from rotation matrix.

Parameters
RRotation matrix.
Returns
rpy Vector of roll-pitch-yaw angles (Euler sequence (0, 1, 2)).

Adapted from formula (289) of "Representing attitude: Euler angles, unit quaternions, and rotation vectors".

◆ rpyFromQuat()

Eigen::Vector3d mc_rbdyn::rpyFromQuat ( const Eigen::Quaterniond &  quat)
inline

Roll-pitch-yaw from unit quaternion.

Parameters
quatInput unit quaternion.
Returns
rpy Vector of roll-pitch-yaw angles (Euler sequence (0, 1, 2)).

Be careful that Eigen's conversion to rotation matrices is the OPPOSITE of that from "Representing attitude: Euler angles, unit quaternions, and rotation vectors", i.e. we need to transpose/flip signs from Equation (290) of this paper.

◆ rpyFromRotation()

template<typename T >
Eigen::Vector3d mc_rbdyn::rpyFromRotation ( const T &  value)

Generic version that can handle a Quaternion or Matrix and returns the RPY angles

◆ rpyToMat() [1/2]

Eigen::Matrix3d mc_rbdyn::rpyToMat ( const double &  r,
const double &  p,
const double &  y 
)
inline

Rotation matrix from roll-pitch-yaw angles.

Parameters
rRoll angle.
pPitch angle.
yYaw angle.
Returns
E Rotation matrix.

◆ rpyToMat() [2/2]

Eigen::Matrix3d mc_rbdyn::rpyToMat ( const Eigen::Vector3d &  rpy)
inline

Rotation matrix from roll-pitch-yaw angles.

Parameters
rpyVector of roll-pitch-yaw angles.
Returns
E Rotation matrix.

◆ rpyToPT() [1/2]

sva::PTransformd mc_rbdyn::rpyToPT ( const double &  r,
const double &  p,
const double &  y 
)
inline

Plucker transfrom from roll-pitch-yaw angles.

Parameters
rRoll angle.
pPitch angle.
yYaw angle.
Returns
X Plucker transform with no translation.

◆ rpyToPT() [2/2]

sva::PTransformd mc_rbdyn::rpyToPT ( const Eigen::Vector3d &  rpy)
inline

Plucker transfrom from roll-pitch-yaw angles.

Parameters
rpyVector of roll-pitch-yaw angles.
Returns
X Plucker transform with no translation.

◆ sch_polyhedron()

MC_RBDYN_DLLAPI sch::S_Object* mc_rbdyn::sch_polyhedron ( const std::vector< sva::PTransformd > &  points)

◆ surface_to_sch()

MC_RBDYN_DLLAPI sch::S_Object* mc_rbdyn::surface_to_sch ( const mc_rbdyn::Surface surface,
const double &  depth = 0.01,
const unsigned int &  slice = 8 
)

◆ urdf_limits_to_bounds()

MC_RBDYN_DLLAPI RobotModule::bounds_t mc_rbdyn::urdf_limits_to_bounds ( const rbd::parsers::Limits &  limits)

Converts limits provided by RBDyn parsers to bounds.

Parameters
limitsLimits as provided by RBDyn parsers

◆ zmp() [1/4]

Eigen::Vector3d MC_RBDYN_DLLAPI mc_rbdyn::zmp ( const sva::ForceVecd &  netTotalWrench,
const Eigen::Vector3d &  plane_p,
const Eigen::Vector3d &  plane_n,
double  minimalNetNormalForce = 1. 
)

Actual ZMP computation from net total wrench and the ZMP plane.

Parameters
netTotalWrenchTotal wrench for all links in contact
plane_pArbitrary point on the ZMP plane
plane_nNormal to the ZMP plane (normalized)
minimalNetNormalForce[N]Mininal force above which the ZMP computation is considered valid. Must be >0 (prevents a divide by zero).
Returns
zmp expressed in the requested plane
Exceptions
Toprevent dividing by zero, throws if the projected force is below minimalNetNormalForce newton. This is highly unlikely to happen and would likely indicate indicate that you are computing a ZMP from invalid forces (such as with the robot in the air).

◆ zmp() [2/4]

Eigen::Vector3d MC_RBDYN_DLLAPI mc_rbdyn::zmp ( const sva::ForceVecd &  netTotalWrench,
const sva::PTransformd &  zmpFrame,
double  minimalNetNormalForce = 1. 
)

ZMP computation from net total wrench and a frame.

See zmpDoc

Parameters
netTotalWrench
zmpFrameFrame used for ZMP computation. The convention here is that the contact frame should have its z-axis pointing in the normal direction of the contact towards the robot.
minimalNetNormalForce[N]Mininal force above which the ZMP computation is considered valid. Must be >0 (prevents a divide by zero).
Exceptions
Toprevent dividing by zero, throws if the projected force is below minimalNetNormalForce newton. This is highly unlikely to happen and would likely indicate indicate that you are computing a ZMP from invalid forces (such as with the robot in the air).
Returns
ZMP expressed in the plane defined by the zmpFrame frame.

◆ zmp() [3/4]

bool MC_RBDYN_DLLAPI mc_rbdyn::zmp ( Eigen::Vector3d &  zmpOut,
const sva::ForceVecd &  netTotalWrench,
const Eigen::Vector3d &  plane_p,
const Eigen::Vector3d &  plane_n,
double  minimalNetNormalForce = 1. 
)
noexcept

Actual ZMP computation from net total wrench and the ZMP plane.

See zmpDoc

Parameters
zmpOutOutput the updated ZMP
netTotalWrenchTotal wrench for all links in contact
plane_pArbitrary point on the ZMP plane
plane_nNormal to the ZMP plane (normalized)
minimalNetNormalForce[N]Mininal force above which the ZMP computation is considered valid. Must be >0 (prevents a divide by zero).
Returns
True if the ZMP was computed, otherwise zmpOut is left as-is and false is returned

◆ zmp() [4/4]

bool MC_RBDYN_DLLAPI mc_rbdyn::zmp ( Eigen::Vector3d &  zmpOut,
const sva::ForceVecd &  netTotalWrench,
const sva::PTransformd &  zmpFrame,
double  minimalNetNormalForce = 1. 
)
noexcept

ZMP computation from net total wrench and a frame.

See zmpDoc

Parameters
zmpOutOutput the updated ZMP
netTotalWrench
zmpFrameFrame used for ZMP computation. The convention here is that the contact frame should have its z-axis pointing in the normal direction of the contact towards the robot.
minimalNetNormalForce[N]Mininal force above which the ZMP computation is considered valid. Must be >0 (prevents a divide by zero).
Returns
True if the ZMP was computed, otherwise zmpOut is left as-is and false is returned