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< RobotModule > | RobotModulePtr |
using | RobotModuleVector = std::vector< RobotModule, Eigen::aligned_allocator< RobotModule > > |
typedef std::shared_ptr< Surface > | SurfacePtr |
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< Plane > | planes_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::Robot & | robotFromConfig (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 ¶ms={}) |
MC_RBDYN_DLLAPI RobotsPtr | loadRobot (const std::string &name, const RobotModule &module, const LoadRobotParameters ¶ms={}) |
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... | |
Forward declarations for the mc_rbdyn library
Interface used to load robots
typedef std::vector<BodySensor, Eigen::aligned_allocator<BodySensor> > mc_rbdyn::BodySensorVector |
using mc_rbdyn::CompoundJointConstraintDescriptionVector = typedef std::vector<CompoundJointConstraintDescription, Eigen::aligned_allocator<CompoundJointConstraintDescription> > |
using mc_rbdyn::ConstFramePtr = typedef std::shared_ptr<const Frame> |
using mc_rbdyn::ConstRobotFramePtr = typedef std::shared_ptr<const RobotFrame> |
using mc_rbdyn::ConstRobotPtr = typedef std::shared_ptr<const Robot> |
using mc_rbdyn::DevicePtr = typedef std::unique_ptr<Device> |
using mc_rbdyn::FramePtr = typedef std::shared_ptr<Frame> |
using mc_rbdyn::Gains2d = typedef Gains<2> |
using mc_rbdyn::Gains3d = typedef Gains<3> |
using mc_rbdyn::Gains6d = typedef Gains<6> |
using mc_rbdyn::RobotDataPtr = typedef std::shared_ptr<RobotData> |
using mc_rbdyn::RobotFramePtr = typedef std::shared_ptr<RobotFrame> |
typedef std::shared_ptr<RobotModule> mc_rbdyn::RobotModulePtr |
using mc_rbdyn::RobotModuleVector = typedef std::vector<RobotModule, Eigen::aligned_allocator<RobotModule> > |
using mc_rbdyn::RobotPtr = typedef std::shared_ptr<Robot> |
using mc_rbdyn::RobotsPtr = typedef std::shared_ptr<Robots> |
using mc_rbdyn::S_ObjectPtr = typedef std::shared_ptr<sch::S_Object> |
using mc_rbdyn::Sensor = typedef Device |
using mc_rbdyn::SensorPtr = typedef DevicePtr |
typedef std::shared_ptr<Surface> mc_rbdyn::SurfacePtr |
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:
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 | ||
) |
MC_RBDYN_DLLAPI sva::PTransformd mc_rbdyn::cylindrical | ( | const double & | T, |
const double & | T_rot | ||
) |
MC_RBDYN_DLLAPI sch::S_Object* mc_rbdyn::cylindrical_hull | ( | const mc_rbdyn::CylindricalSurface & | surface, |
const unsigned int & | slice | ||
) |
MC_RBDYN_DLLAPI void mc_rbdyn::cylindricalParam | ( | const sva::PTransformd & | X_es_rs, |
double & | T, | ||
double & | T_rot | ||
) |
MC_RBDYN_DLLAPI sch::S_Object* mc_rbdyn::gripper_hull | ( | const mc_rbdyn::GripperSurface & | surface, |
const double & | depth | ||
) |
|
inline |
MC_RBDYN_DLLAPI std::vector<double> mc_rbdyn::jointParam | ( | const Surface & | r1Surface, |
const Surface & | r2Surface, | ||
const sva::PTransformd & | X_es_rs | ||
) |
MC_RBDYN_DLLAPI RobotsPtr mc_rbdyn::loadRobot | ( | const RobotModule & | module, |
const LoadRobotParameters & | params = {} |
||
) |
MC_RBDYN_DLLAPI RobotsPtr mc_rbdyn::loadRobot | ( | const std::string & | name, |
const RobotModule & | module, | ||
const LoadRobotParameters & | params = {} |
||
) |
MC_RBDYN_DLLAPI RobotsPtr mc_rbdyn::loadRobotAndEnv | ( | const RobotModule & | module, |
const RobotModule & | envModule | ||
) |
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 = {} |
||
) |
MC_RBDYN_DLLAPI RobotsPtr mc_rbdyn::loadRobots | ( | const std::vector< std::shared_ptr< RobotModule >> & | modules | ) |
MC_RBDYN_DLLAPI std::ostream& mc_rbdyn::operator<< | ( | std::ostream & | os, |
const Collision & | c | ||
) |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
MC_RBDYN_DLLAPI sva::PTransformd mc_rbdyn::planar | ( | const double & | T, |
const double & | B, | ||
const double & | N_rot | ||
) |
MC_RBDYN_DLLAPI sch::S_Object* mc_rbdyn::planar_hull | ( | const mc_rbdyn::PlanarSurface & | surface, |
const double & | depth | ||
) |
MC_RBDYN_DLLAPI void mc_rbdyn::planarParam | ( | const sva::PTransformd & | X_es_rs, |
double & | T, | ||
double & | B, | ||
double & | N_rot | ||
) |
MC_RBDYN_DLLAPI std::vector<Plane> mc_rbdyn::planes_from_polygon | ( | const std::shared_ptr< geos::geom::Geometry > & | geometry | ) |
MC_RBDYN_DLLAPI std::vector<Eigen::Vector3d> mc_rbdyn::points_from_polygon | ( | std::shared_ptr< geos::geom::Geometry > | geometry | ) |
MC_RBDYN_DLLAPI std::vector<std::shared_ptr<Surface> > mc_rbdyn::readRSDFFromDir | ( | const std::string & | dirname | ) |
|
inline |
Roll-pitch-yaw from rotation matrix.
R | Rotation matrix. |
Adapted from formula (289) of "Representing attitude: Euler angles, unit quaternions, and rotation vectors".
|
inline |
Roll-pitch-yaw from unit quaternion.
quat | Input unit quaternion. |
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.
Eigen::Vector3d mc_rbdyn::rpyFromRotation | ( | const T & | value | ) |
Generic version that can handle a Quaternion or Matrix and returns the RPY angles
|
inline |
Rotation matrix from roll-pitch-yaw angles.
r | Roll angle. |
p | Pitch angle. |
y | Yaw angle. |
|
inline |
Rotation matrix from roll-pitch-yaw angles.
rpy | Vector of roll-pitch-yaw angles. |
|
inline |
Plucker transfrom from roll-pitch-yaw angles.
r | Roll angle. |
p | Pitch angle. |
y | Yaw angle. |
|
inline |
Plucker transfrom from roll-pitch-yaw angles.
rpy | Vector of roll-pitch-yaw angles. |
MC_RBDYN_DLLAPI sch::S_Object* mc_rbdyn::sch_polyhedron | ( | const std::vector< sva::PTransformd > & | points | ) |
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 |
||
) |
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.
limits | Limits as provided by RBDyn parsers |
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.
netTotalWrench | Total wrench for all links in contact |
plane_p | Arbitrary point on the ZMP plane |
plane_n | Normal 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). |
To | prevent 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). |
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
netTotalWrench | |
zmpFrame | Frame 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). |
To | prevent 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). |
|
noexcept |
Actual ZMP computation from net total wrench and the ZMP plane.
See zmpDoc
zmpOut | Output the updated ZMP |
netTotalWrench | Total wrench for all links in contact |
plane_p | Arbitrary point on the ZMP plane |
plane_n | Normal 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). |
zmpOut
is left as-is and false is returned
|
noexcept |
ZMP computation from net total wrench and a frame.
See zmpDoc
zmpOut | Output the updated ZMP |
netTotalWrench | |
zmpFrame | Frame 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). |
zmpOut
is left as-is and false is returned