The graph which contains all information taken from the mjcf file. More...
#include <mjcf-graph.hpp>
Public Types | |
typedef std::unordered_map< std::string, MjcfBody > | BodyMap_t |
typedef std::unordered_map< std::string, MjcfClass > | ClassMap_t |
typedef std::unordered_map< std::string, Eigen::VectorXd > | ConfigMap_t |
typedef std::map< std::string, MjcfEquality > | EqualityMap_t |
typedef std::unordered_map< std::string, MjcfMaterial > | MaterialMap_t |
typedef std::unordered_map< std::string, MjcfMesh > | MeshMap_t |
typedef boost::property_tree::ptree | ptree |
typedef std::unordered_map< std::string, MjcfTexture > | TextureMap_t |
typedef pinocchio::urdf::details::UrdfVisitor< double, 0, ::pinocchio::JointCollectionDefaultTpl > | UrdfVisitor |
typedef std::vector< std::string > | VectorOfStrings |
Public Member Functions | |
void | addKeyFrame (const Eigen::VectorXd &keyframe, const std::string &keyName) |
Add a keyframe to the model (ie reference configuration) More... | |
void | addSoloJoint (const MjcfJoint &jointInfo, const MjcfBody ¤tBody, SE3 &bodyInJoint) |
Add a joint to the model. only needed when a body has a solo joint child. More... | |
Inertia | convertInertiaFromMjcf (const ptree &el) const |
Convert Inertia of an mjcf element into Inertia model of pinocchio. More... | |
SE3 | convertPosition (const ptree &el) const |
Convert pose of an mjcf element into SE3. More... | |
template<typename TypeX , typename TypeY , typename TypeZ , typename TypeUnaligned > | |
JointModel | createJoint (const Eigen::Vector3d &axis) |
Create a joint to add to the joint composite if needed. More... | |
void | fillModel (const std::string &nameOfBody) |
Use all the infos that were parsed from the xml file to add a body and joint to the model. More... | |
void | fillReferenceConfig (const MjcfBody ¤tBody) |
Fill reference configuration for a body and all it's associated dof. More... | |
MjcfGraph (UrdfVisitor &urdfVisitor, const std::string &modelPath) | |
graph constructor More... | |
void | parseAsset (const ptree &el) |
Parse all the info from the meta tag asset (mesh, material, texture) More... | |
void | parseCompiler (const ptree &el) |
Parse all the info from the compile node into compilerInfo. More... | |
void | parseContactInformation (const Model &model, PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) &contact_models) |
Parse the equality constraints and add them to the model. More... | |
void | parseDefault (ptree &el, const ptree &parent) |
Go through the default part of the file and get all the class name. Fill the mapOfDefault for later use. More... | |
void | parseEquality (const ptree &el) |
Parse all the info from the equality tag. More... | |
void | parseGeomTree (const GeometryType &type, GeometryModel &geomModel, ::hpp::fcl::MeshLoaderPtr &meshLoader) |
Fill geometry model with all the info taken from the mjcf model file. More... | |
void | parseGraph () |
parse the mjcf file into a graph More... | |
void | parseGraphFromXML (const std::string &xmlStr) |
parse the mjcf file into a graph More... | |
void | parseJointAndBody (const ptree &el, const boost::optional< std::string > &childClass, const std::string &parentName="") |
Go through the main body of the mjcf file "worldbody" to get all the info ready to create the model. More... | |
void | parseKeyFrame (const ptree &el) |
Parse all the info from the meta tag keyframe. More... | |
void | parseMaterial (const ptree &el) |
Parse all the info from a material node. More... | |
void | parseMesh (const ptree &el) |
Parse all the info from a mesh node. More... | |
void | parseRootTree () |
Fill the pinocchio model with all the infos from the graph. More... | |
void | parseTexture (const ptree &el) |
Parse all the info from a texture node. More... | |
Public Attributes | |
VectorOfStrings | bodiesList |
MjcfCompiler | compilerInfo |
BodyMap_t | mapOfBodies |
ClassMap_t | mapOfClasses |
ConfigMap_t | mapOfConfigs |
EqualityMap_t | mapOfEqualities |
MaterialMap_t | mapOfMaterials |
MeshMap_t | mapOfMeshes |
TextureMap_t | mapOfTextures |
std::string | modelName |
std::string | modelPath |
ptree | pt |
Eigen::VectorXd | referenceConfig |
UrdfVisitor & | urdfVisitor |
The graph which contains all information taken from the mjcf file.
Definition at line 379 of file mjcf-graph.hpp.
typedef std::unordered_map<std::string, MjcfBody> pinocchio::mjcf::details::MjcfGraph::BodyMap_t |
Definition at line 384 of file mjcf-graph.hpp.
typedef std::unordered_map<std::string, MjcfClass> pinocchio::mjcf::details::MjcfGraph::ClassMap_t |
Definition at line 385 of file mjcf-graph.hpp.
typedef std::unordered_map<std::string, Eigen::VectorXd> pinocchio::mjcf::details::MjcfGraph::ConfigMap_t |
Definition at line 389 of file mjcf-graph.hpp.
typedef std::map<std::string, MjcfEquality> pinocchio::mjcf::details::MjcfGraph::EqualityMap_t |
Definition at line 390 of file mjcf-graph.hpp.
typedef std::unordered_map<std::string, MjcfMaterial> pinocchio::mjcf::details::MjcfGraph::MaterialMap_t |
Definition at line 386 of file mjcf-graph.hpp.
typedef std::unordered_map<std::string, MjcfMesh> pinocchio::mjcf::details::MjcfGraph::MeshMap_t |
Definition at line 387 of file mjcf-graph.hpp.
typedef boost::property_tree::ptree pinocchio::mjcf::details::MjcfGraph::ptree |
Definition at line 382 of file mjcf-graph.hpp.
typedef std::unordered_map<std::string, MjcfTexture> pinocchio::mjcf::details::MjcfGraph::TextureMap_t |
Definition at line 388 of file mjcf-graph.hpp.
typedef pinocchio::urdf::details:: UrdfVisitor<double, 0, ::pinocchio::JointCollectionDefaultTpl> pinocchio::mjcf::details::MjcfGraph::UrdfVisitor |
Definition at line 425 of file mjcf-graph.hpp.
typedef std::vector<std::string> pinocchio::mjcf::details::MjcfGraph::VectorOfStrings |
Definition at line 383 of file mjcf-graph.hpp.
|
inline |
void pinocchio::mjcf::details::MjcfGraph::addKeyFrame | ( | const Eigen::VectorXd & | keyframe, |
const std::string & | keyName | ||
) |
Add a keyframe to the model (ie reference configuration)
keyframe | Keyframe to add |
keyName | Name of the keyframe |
Definition at line 1044 of file mjcf-graph.cpp.
void pinocchio::mjcf::details::MjcfGraph::addSoloJoint | ( | const MjcfJoint & | jointInfo, |
const MjcfBody & | currentBody, | ||
SE3 & | bodyInJoint | ||
) |
Add a joint to the model. only needed when a body has a solo joint child.
jointInfo | The joint to add to the tree |
currentBody | The body associated with the joint |
bodyInJoint | Position of the body wrt to its joint |
Definition at line 851 of file mjcf-graph.cpp.
Convert Inertia of an mjcf element into Inertia model of pinocchio.
el | ptree element with all the inertial information |
Definition at line 346 of file mjcf-graph.cpp.
Convert pose of an mjcf element into SE3.
el | ptree element with all the pose element |
Definition at line 275 of file mjcf-graph.cpp.
JointModel pinocchio::mjcf::details::MjcfGraph::createJoint | ( | const Eigen::Vector3d & | axis | ) |
Create a joint to add to the joint composite if needed.
TypeX | joint with axis X |
TypeY | joint with axis Y |
TypeZ | joint with axis Z |
TypeUnaligned | joint with axis unaligned |
axis | axis of the joint |
Definition at line 839 of file mjcf-graph.cpp.
void pinocchio::mjcf::details::MjcfGraph::fillModel | ( | const std::string & | nameOfBody | ) |
Use all the infos that were parsed from the xml file to add a body and joint to the model.
nameOfBody | Name of the body to add |
Definition at line 904 of file mjcf-graph.cpp.
void pinocchio::mjcf::details::MjcfGraph::fillReferenceConfig | ( | const MjcfBody & | currentBody | ) |
Fill reference configuration for a body and all it's associated dof.
currentBody | body to check |
Definition at line 1022 of file mjcf-graph.cpp.
void pinocchio::mjcf::details::MjcfGraph::parseAsset | ( | const ptree & | el | ) |
Parse all the info from the meta tag asset (mesh, material, texture)
el | ptree texture node |
Definition at line 585 of file mjcf-graph.cpp.
void pinocchio::mjcf::details::MjcfGraph::parseCompiler | ( | const ptree & | el | ) |
Parse all the info from the compile node into compilerInfo.
el | ptree compile node |
Definition at line 627 of file mjcf-graph.cpp.
void pinocchio::mjcf::details::MjcfGraph::parseContactInformation | ( | const Model & | model, |
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & | contact_models | ||
) |
Parse the equality constraints and add them to the model.
model | Model to add the constraints to |
contact_models | Vector of contact models to add the constraints to |
Definition at line 1097 of file mjcf-graph.cpp.
Go through the default part of the file and get all the class name. Fill the mapOfDefault for later use.
el | ptree element. Root of the default |
Definition at line 603 of file mjcf-graph.cpp.
void pinocchio::mjcf::details::MjcfGraph::parseEquality | ( | const ptree & | el | ) |
Parse all the info from the equality tag.
el | ptree equality node |
Definition at line 740 of file mjcf-graph.cpp.
void pinocchio::mjcf::details::MjcfGraph::parseGeomTree | ( | const GeometryType & | type, |
GeometryModel & | geomModel, | ||
::hpp::fcl::MeshLoaderPtr & | meshLoader | ||
) |
Fill geometry model with all the info taken from the mjcf model file.
type | Type of geometry to parse (COLLISION or VISUAL) |
geomModel | geometry model to fill |
meshLoader | mesh loader from hpp::fcl |
Definition at line 500 of file mjcf-graph-geom.cpp.
void pinocchio::mjcf::details::MjcfGraph::parseGraph | ( | ) |
parse the mjcf file into a graph
Definition at line 787 of file mjcf-graph.cpp.
void pinocchio::mjcf::details::MjcfGraph::parseGraphFromXML | ( | const std::string & | xmlStr | ) |
parse the mjcf file into a graph
xmlStr | xml file name |
Definition at line 832 of file mjcf-graph.cpp.
void pinocchio::mjcf::details::MjcfGraph::parseJointAndBody | ( | const ptree & | el, |
const boost::optional< std::string > & | childClass, | ||
const std::string & | parentName = "" |
||
) |
Go through the main body of the mjcf file "worldbody" to get all the info ready to create the model.
el | root of the tree |
parentName | name of the parentBody in the robot tree |
Definition at line 394 of file mjcf-graph.cpp.
void pinocchio::mjcf::details::MjcfGraph::parseKeyFrame | ( | const ptree & | el | ) |
Parse all the info from the meta tag keyframe.
el | ptree keyframe node |
Definition at line 719 of file mjcf-graph.cpp.
void pinocchio::mjcf::details::MjcfGraph::parseMaterial | ( | const ptree & | el | ) |
Parse all the info from a material node.
el | ptree material node |
Definition at line 538 of file mjcf-graph.cpp.
void pinocchio::mjcf::details::MjcfGraph::parseMesh | ( | const ptree & | el | ) |
Parse all the info from a mesh node.
el | ptree mesh node |
Definition at line 563 of file mjcf-graph.cpp.
void pinocchio::mjcf::details::MjcfGraph::parseRootTree | ( | ) |
Fill the pinocchio model with all the infos from the graph.
Definition at line 1128 of file mjcf-graph.cpp.
void pinocchio::mjcf::details::MjcfGraph::parseTexture | ( | const ptree & | el | ) |
Parse all the info from a texture node.
el | ptree texture node |
Definition at line 500 of file mjcf-graph.cpp.
VectorOfStrings pinocchio::mjcf::details::MjcfGraph::bodiesList |
Definition at line 416 of file mjcf-graph.hpp.
MjcfCompiler pinocchio::mjcf::details::MjcfGraph::compilerInfo |
Definition at line 393 of file mjcf-graph.hpp.
BodyMap_t pinocchio::mjcf::details::MjcfGraph::mapOfBodies |
Definition at line 397 of file mjcf-graph.hpp.
ClassMap_t pinocchio::mjcf::details::MjcfGraph::mapOfClasses |
Definition at line 395 of file mjcf-graph.hpp.
ConfigMap_t pinocchio::mjcf::details::MjcfGraph::mapOfConfigs |
Definition at line 405 of file mjcf-graph.hpp.
EqualityMap_t pinocchio::mjcf::details::MjcfGraph::mapOfEqualities |
Definition at line 407 of file mjcf-graph.hpp.
MaterialMap_t pinocchio::mjcf::details::MjcfGraph::mapOfMaterials |
Definition at line 399 of file mjcf-graph.hpp.
MeshMap_t pinocchio::mjcf::details::MjcfGraph::mapOfMeshes |
Definition at line 401 of file mjcf-graph.hpp.
TextureMap_t pinocchio::mjcf::details::MjcfGraph::mapOfTextures |
Definition at line 403 of file mjcf-graph.hpp.
std::string pinocchio::mjcf::details::MjcfGraph::modelName |
Definition at line 419 of file mjcf-graph.hpp.
std::string pinocchio::mjcf::details::MjcfGraph::modelPath |
Definition at line 420 of file mjcf-graph.hpp.
ptree pinocchio::mjcf::details::MjcfGraph::pt |
Definition at line 413 of file mjcf-graph.hpp.
Eigen::VectorXd pinocchio::mjcf::details::MjcfGraph::referenceConfig |
Definition at line 410 of file mjcf-graph.hpp.
UrdfVisitor& pinocchio::mjcf::details::MjcfGraph::urdfVisitor |
Definition at line 426 of file mjcf-graph.hpp.