From 55018ee3c50e7a65bff91fa6131349923e403c4b Mon Sep 17 00:00:00 2001 From: Marcos Wagner Date: Thu, 22 Jul 2021 10:59:41 -0300 Subject: [PATCH] Addressing PR comments Signed-off-by: Marcos Wagner --- src/ModelCommandAPI_TEST.cc | 147 +++++++++++++++-------- src/cmd/ModelCommandAPI.cc | 88 +++++++++----- src/cmd/ModelCommandAPI.hh | 4 +- src/cmd/cmdmodel.rb.in | 29 ++--- tutorials/model_command.md | 224 +++++++++++++++++++----------------- 5 files changed, 295 insertions(+), 197 deletions(-) diff --git a/src/ModelCommandAPI_TEST.cc b/src/ModelCommandAPI_TEST.cc index ca4d7c01e74..1a0b7108c5c 100644 --- a/src/ModelCommandAPI_TEST.cc +++ b/src/ModelCommandAPI_TEST.cc @@ -117,72 +117,93 @@ TEST(ModelCommandAPI, Commands) "\nRequesting state for world [diff_drive]...\n\n" "Model: [8]\n" " - Name: vehicle_blue\n" - " - Pose: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" " [0.000000 | 2.000000 | 0.325000]\n" " [0.000000 | 0.000000 | 0.000000]\n" "\n" " - Link [9]\n" " - Name: chassis\n" " - Parent: vehicle_blue [8]\n" - " - Mass: [1.143950]\n" - " - Inertial Matrix: \n" + " - Mass (kg): [1.143950]\n" + " - Inertial Pose:\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Inertial Matrix (kg⋅m^2): \n" " [0.126164 | 0.000000 | 0.000000]\n" " [0.000000 | 0.416519 | 0.000000]\n" " [0.000000 | 0.000000 | 0.481014]\n" - " - Pose: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" " [-0.151427 | 0.000000 | 0.175000]\n" " [0.000000 | 0.000000 | 0.000000]\n" " - Link [12]\n" " - Name: left_wheel\n" " - Parent: vehicle_blue [8]\n" - " - Mass: [2.000000]\n" - " - Inertial Matrix: \n" + " - Mass (kg): [2.000000]\n" + " - Inertial Pose:\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Inertial Matrix (kg⋅m^2): \n" " [0.145833 | 0.000000 | 0.000000]\n" " [0.000000 | 0.145833 | 0.000000]\n" " [0.000000 | 0.000000 | 0.125000]\n" - " - Pose: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" " [0.554283 | 0.625029 | -0.025000]\n" " [-1.570700 | 0.000000 | 0.000000]\n" " - Link [15]\n" " - Name: right_wheel\n" " - Parent: vehicle_blue [8]\n" - " - Mass: [2.000000]\n" - " - Inertial Matrix: \n" + " - Mass (kg): [2.000000]\n" + " - Inertial Pose:\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Inertial Matrix (kg⋅m^2): \n" " [0.145833 | 0.000000 | 0.000000]\n" " [0.000000 | 0.145833 | 0.000000]\n" " [0.000000 | 0.000000 | 0.125000]\n" - " - Pose: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" " [0.554282 | -0.625029 | -0.025000]\n" " [-1.570700 | 0.000000 | 0.000000]\n" " - Link [18]\n" " - Name: caster\n" " - Parent: vehicle_blue [8]\n" - " - Mass: [1.000000]\n" - " - Inertial Matrix: \n" + " - Mass (kg): [1.000000]\n" + " - Inertial Pose:\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Inertial Matrix (kg⋅m^2): \n" " [0.100000 | 0.000000 | 0.000000]\n" " [0.000000 | 0.100000 | 0.000000]\n" " [0.000000 | 0.000000 | 0.100000]\n" - " - Pose: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" " [-0.957138 | 0.000000 | -0.125000]\n" " [0.000000 | 0.000000 | 0.000000]\n" " - Joint [21]\n" " - Name: left_wheel_joint\n" " - Parent: vehicle_blue [8]\n" - " - Joint type: revolute\n" - " - Parent Link: [left_wheel]\n" - " - Child Link: [chassis]\n" + " - Type: revolute\n" + " - Parent Link: left_wheel\n" + " - Child Link: chassis\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" + " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Axis unit vector [ XYZ ]: \n" + " [0 | 0 | 1]\n" " - Joint [22]\n" " - Name: right_wheel_joint\n" " - Parent: vehicle_blue [8]\n" - " - Joint type: revolute\n" - " - Parent Link: [right_wheel]\n" - " - Child Link: [chassis]\n" + " - Type: revolute\n" + " - Parent Link: right_wheel\n" + " - Child Link: chassis\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" + " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Axis unit vector [ XYZ ]: \n" + " [0 | 0 | 1]\n" " - Joint [23]\n" " - Name: caster_wheel\n" " - Parent: vehicle_blue [8]\n" - " - Joint type: ball\n" - " - Parent Link: [caster]\n" - " - Child Link: [chassis]\n"; + " - Type: ball\n" + " - Parent Link: caster\n" + " - Child Link: chassis\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" + " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 | 0.000000 | 0.000000]\n"; EXPECT_EQ(expectedOutput, output); } @@ -195,7 +216,7 @@ TEST(ModelCommandAPI, Commands) "\nRequesting state for world [diff_drive]...\n\n" "Model: [8]\n" " - Name: vehicle_blue\n" - " - Pose: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" " [0.000000 | 2.000000 | 0.325000]\n" " [0.000000 | 0.000000 | 0.000000]\n\n"; EXPECT_EQ(expectedOutput, output); @@ -212,45 +233,53 @@ TEST(ModelCommandAPI, Commands) " - Link [9]\n" " - Name: chassis\n" " - Parent: vehicle_blue [8]\n" - " - Mass: [1.143950]\n" - " - Inertial Matrix: \n" + " - Mass (kg): [1.143950]\n" + " - Inertial Pose:\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Inertial Matrix (kg⋅m^2): \n" " [0.126164 | 0.000000 | 0.000000]\n" " [0.000000 | 0.416519 | 0.000000]\n" " [0.000000 | 0.000000 | 0.481014]\n" - " - Pose: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" " [-0.151427 | 0.000000 | 0.175000]\n" " [0.000000 | 0.000000 | 0.000000]\n" " - Link [12]\n" " - Name: left_wheel\n" " - Parent: vehicle_blue [8]\n" - " - Mass: [2.000000]\n" - " - Inertial Matrix: \n" + " - Mass (kg): [2.000000]\n" + " - Inertial Pose:\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Inertial Matrix (kg⋅m^2): \n" " [0.145833 | 0.000000 | 0.000000]\n" " [0.000000 | 0.145833 | 0.000000]\n" " [0.000000 | 0.000000 | 0.125000]\n" - " - Pose: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" " [0.554283 | 0.625029 | -0.025000]\n" " [-1.570700 | 0.000000 | 0.000000]\n" " - Link [15]\n" " - Name: right_wheel\n" " - Parent: vehicle_blue [8]\n" - " - Mass: [2.000000]\n" - " - Inertial Matrix: \n" + " - Mass (kg): [2.000000]\n" + " - Inertial Pose:\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Inertial Matrix (kg⋅m^2): \n" " [0.145833 | 0.000000 | 0.000000]\n" " [0.000000 | 0.145833 | 0.000000]\n" " [0.000000 | 0.000000 | 0.125000]\n" - " - Pose: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" " [0.554282 | -0.625029 | -0.025000]\n" " [-1.570700 | 0.000000 | 0.000000]\n" " - Link [18]\n" " - Name: caster\n" " - Parent: vehicle_blue [8]\n" - " - Mass: [1.000000]\n" - " - Inertial Matrix: \n" + " - Mass (kg): [1.000000]\n" + " - Inertial Pose:\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Inertial Matrix (kg⋅m^2): \n" " [0.100000 | 0.000000 | 0.000000]\n" " [0.000000 | 0.100000 | 0.000000]\n" " [0.000000 | 0.000000 | 0.100000]\n" - " - Pose: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" " [-0.957138 | 0.000000 | -0.125000]\n" " [0.000000 | 0.000000 | 0.000000]\n"; EXPECT_EQ(expectedOutput, output); @@ -267,12 +296,14 @@ TEST(ModelCommandAPI, Commands) " - Link [18]\n" " - Name: caster\n" " - Parent: vehicle_blue [8]\n" - " - Mass: [1.000000]\n" - " - Inertial Matrix: \n" + " - Mass (kg): [1.000000]\n" + " - Inertial Pose:\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Inertial Matrix (kg⋅m^2): \n" " [0.100000 | 0.000000 | 0.000000]\n" " [0.000000 | 0.100000 | 0.000000]\n" " [0.000000 | 0.000000 | 0.100000]\n" - " - Pose: \n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" " [-0.957138 | 0.000000 | -0.125000]\n" " [0.000000 | 0.000000 | 0.000000]\n"; EXPECT_EQ(expectedOutput, output); @@ -289,21 +320,34 @@ TEST(ModelCommandAPI, Commands) " - Joint [21]\n" " - Name: left_wheel_joint\n" " - Parent: vehicle_blue [8]\n" - " - Joint type: revolute\n" - " - Parent Link: [left_wheel]\n" - " - Child Link: [chassis]\n" + " - Type: revolute\n" + " - Parent Link: left_wheel\n" + " - Child Link: chassis\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" + " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Axis unit vector [ XYZ ]: \n" + " [0 | 0 | 1]\n" " - Joint [22]\n" " - Name: right_wheel_joint\n" " - Parent: vehicle_blue [8]\n" - " - Joint type: revolute\n" - " - Parent Link: [right_wheel]\n" - " - Child Link: [chassis]\n" + " - Type: revolute\n" + " - Parent Link: right_wheel\n" + " - Child Link: chassis\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" + " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 | 0.000000 | 0.000000]\n" + " - Axis unit vector [ XYZ ]: \n" + " [0 | 0 | 1]\n" " - Joint [23]\n" " - Name: caster_wheel\n" " - Parent: vehicle_blue [8]\n" - " - Joint type: ball\n" - " - Parent Link: [caster]\n" - " - Child Link: [chassis]\n"; + " - Type: ball\n" + " - Parent Link: caster\n" + " - Child Link: chassis\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" + " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 | 0.000000 | 0.000000]\n"; EXPECT_EQ(expectedOutput, output); } @@ -318,9 +362,12 @@ TEST(ModelCommandAPI, Commands) " - Joint [23]\n" " - Name: caster_wheel\n" " - Parent: vehicle_blue [8]\n" - " - Joint type: ball\n" - " - Parent Link: [caster]\n" - " - Child Link: [chassis]\n"; + " - Type: ball\n" + " - Parent Link: caster\n" + " - Child Link: chassis\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]: \n" + " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 | 0.000000 | 0.000000]\n"; EXPECT_EQ(expectedOutput, output); } } diff --git a/src/cmd/ModelCommandAPI.cc b/src/cmd/ModelCommandAPI.cc index 630e73a9ab4..d6ba45e8ad8 100644 --- a/src/cmd/ModelCommandAPI.cc +++ b/src/cmd/ModelCommandAPI.cc @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -145,7 +146,8 @@ void printPose(const uint64_t _entity, std::cout << "Model: [" << _entity << "]" << std::endl << " - Name: " << nameComp->Data() << std::endl - << " - Pose: " << poseInfo << std::endl << std::endl; + << " - Pose [ XYZ (m) ] [ RPY (rad) ]: " + << poseInfo << std::endl << std::endl; } } @@ -161,7 +163,7 @@ void printLinks(const uint64_t _entity, const auto links = _ecm.EntitiesByComponents( ignition::gazebo::components::ParentEntity(_entity), ignition::gazebo::components::Link()); - for(const auto &entity : links) + for (const auto &entity : links) { const auto parentComp = _ecm.Component(entity); @@ -169,7 +171,7 @@ void printLinks(const uint64_t _entity, const auto nameComp = _ecm.Component(entity); - if(_linkName.length() && _linkName != nameComp->Data()) + if (_linkName.length() && _linkName != nameComp->Data()) continue; if (parentComp) @@ -197,6 +199,11 @@ void printLinks(const uint64_t _entity, const auto inertialMatrix = inertialComp->Data().MassMatrix(); const auto massComp = inertialComp->Data().MassMatrix().Mass(); + const std::string inertialPose = + "\n [" + std::to_string(inertialComp->Data().Pose().X()) + " | " + + std::to_string(inertialComp->Data().Pose().Y()) + " | " + + std::to_string(inertialComp->Data().Pose().Z()) + "]"; + const std::string massInfo = "[" + std::to_string(massComp) + "]"; const std::string inertialInfo = "\n [" + std::to_string(inertialMatrix.Ixx()) + " | " @@ -208,9 +215,16 @@ void printLinks(const uint64_t _entity, " [" + std::to_string(inertialMatrix.Ixz()) + " | " + std::to_string(inertialMatrix.Iyz()) + " | " + std::to_string(inertialMatrix.Izz()) + "]"; - std::cout << " - Mass: " << massInfo << std::endl - << " - Inertial Matrix: " << inertialInfo << std::endl; + std::cout << " - Mass (kg): " << massInfo << std::endl + << " - Inertial Pose:" << inertialPose << std::endl + << " - Inertial Matrix (kg⋅m^2): " + << inertialInfo << std::endl; } + const std::string inertialPose = + "\n [" + std::to_string(inertialComp->Data().Pose().X()) + " | " + + std::to_string(inertialComp->Data().Pose().Y()) + " | " + + std::to_string(inertialComp->Data().Pose().Z()) + "]"; + const auto poseComp = _ecm.Component(entity); @@ -225,7 +239,8 @@ void printLinks(const uint64_t _entity, + std::to_string(poseComp->Data().Pitch()) + " | " + std::to_string(poseComp->Data().Yaw()) + "]"; - std::cout << " - Pose: " << poseInfo << std::endl; + std::cout << " - Pose [ XYZ (m) ] [ RPY (rad) ]: " + << poseInfo << std::endl; } } } @@ -255,7 +270,7 @@ void printJoints(const uint64_t entity, ignition::gazebo::components::ParentEntity(entity), ignition::gazebo::components::Joint()); - for(const auto &_entity : joints) + for (const auto &_entity : joints) { const auto parentComp = _ecm.Component(_entity); @@ -263,7 +278,7 @@ void printJoints(const uint64_t entity, const auto nameComp = _ecm.Component(_entity); - if(_jointName.length() && _jointName != nameComp->Data()) + if (_jointName.length() && _jointName != nameComp->Data()) continue; if (parentComp) @@ -290,12 +305,33 @@ void printJoints(const uint64_t entity, _ecm.Component(_entity); const auto parentLinkComp = _ecm.Component(_entity); + const auto poseComp = + _ecm.Component(_entity); + const auto axisComp = + _ecm.Component(_entity); if (childLinkComp && parentLinkComp) { - std::cout << " - Joint type: " << jointTypes.at(jointComp->Data()) - << "\n" << " - Parent Link: [" << childLinkComp->Data() << "]\n" - << " - Child Link: [" << parentLinkComp->Data() << "]\n"; + const std::string poseInfo = + "\n [" + std::to_string(poseComp->Data().X()) + " | " + + std::to_string(poseComp->Data().Y()) + " | " + + std::to_string(poseComp->Data().Z()) + "]\n" + " [" + std::to_string(poseComp->Data().Roll()) + " | " + + std::to_string(poseComp->Data().Pitch()) + " | " + + std::to_string(poseComp->Data().Yaw()) + "]"; + + std::cout << " - Type: " << jointTypes.at(jointComp->Data()) + << "\n" << " - Parent Link: " << childLinkComp->Data() << "\n" + << " - Child Link: " << parentLinkComp->Data() << "\n" + << " - Pose [ XYZ (m) ] [ RPY (rad) ]: " + << poseInfo << std::endl; + } + if (axisComp) + { + std::cout << " - Axis unit vector [ XYZ ]: \n" + " [" << axisComp->Data().Xyz().X() << " | " + << axisComp->Data().Xyz().Y() << " | " + << axisComp->Data().Xyz().Z() << "]\n"; } } } @@ -305,7 +341,7 @@ void printJoints(const uint64_t entity, extern "C" IGNITION_GAZEBO_VISIBLE void cmdModelList() { ignition::gazebo::EntityComponentManager ecm{}; - if(!populateECM(ecm)) + if (!populateECM(ecm)) { return; } @@ -315,7 +351,7 @@ extern "C" IGNITION_GAZEBO_VISIBLE void cmdModelList() std::cout << "Available models:" << std::endl; - for(const auto &m : models) + for (const auto &m : models) { const auto nameComp = ecm.Component(m); @@ -325,30 +361,27 @@ extern "C" IGNITION_GAZEBO_VISIBLE void cmdModelList() ////////////////////////////////////////////////// extern "C" IGNITION_GAZEBO_VISIBLE void cmdModelInfo( - const char *_model, int _pose, int _link, const char *_linkName, - int _joint, const char *_joint_name) + const char *_model, int _pose, const char *_linkName, const char *_jointName) { std::string linkName{""}; - if(_linkName) + if (_linkName) linkName = _linkName; std::string jointName{""}; - if(_joint_name) - jointName = _joint_name; - + if (_jointName) + jointName = _jointName; bool printAll{false}; - if(!_pose && !_link && !_joint) + if (!_pose && !_linkName && !_jointName) printAll = true; - if(!_model) + if (!_model) { std::cerr << std::endl << "Model name not found" << std::endl; return; } const std::string model{_model}; - // Get arguments ignition::gazebo::EntityComponentManager ecm{}; - if(!populateECM(ecm)) + if (!populateECM(ecm)) return; // Get the desired model entity. @@ -356,15 +389,18 @@ extern "C" IGNITION_GAZEBO_VISIBLE void cmdModelInfo( ignition::gazebo::components::Name(model), ignition::gazebo::components::Model()); + if (entity == ignition::gazebo::kNullEntity) + std::cout << "No model named <" << model << "> was found" << std::endl; + // Get the pose of the model - if(printAll | _pose) + if (printAll | _pose) printPose(entity, ecm); // Get the links information - if(printAll | _link) + if (printAll | (_linkName != nullptr)) printLinks(entity, ecm, linkName); // Get the links information - if(printAll | _joint) + if (printAll | (_jointName != nullptr)) printJoints(entity, ecm, jointName); } diff --git a/src/cmd/ModelCommandAPI.hh b/src/cmd/ModelCommandAPI.hh index b42cf7cf593..072ab9c55e6 100644 --- a/src/cmd/ModelCommandAPI.hh +++ b/src/cmd/ModelCommandAPI.hh @@ -32,6 +32,6 @@ extern "C" IGNITION_GAZEBO_VISIBLE void cmdModelList(); /// \param[in] _joint --joint option. /// \param[in] _joint_name Joint name. extern "C" IGNITION_GAZEBO_VISIBLE void cmdModelInfo( - const char *_model, int _pose, int _link, const char *_linkName, - int _joint, const char *_jointName); + const char *_model, int _pose, const char *_linkName, + const char *_jointName); diff --git a/src/cmd/cmdmodel.rb.in b/src/cmd/cmdmodel.rb.in index 967c679c3b1..0351b240876 100644 --- a/src/cmd/cmdmodel.rb.in +++ b/src/cmd/cmdmodel.rb.in @@ -43,6 +43,7 @@ COMMANDS = { 'model' => "Available Options: \n"\ " --list Get a list of the available models. \n"\ " -m [--model] arg Select the model to be shown. \n"\ + " -p [--pose] Print the pose of the model. \n"\ " -l [--link] arg Select a link to show its properties. \n"\ " If no arg is passed all links are printed \n"\ " Requires the -m option \n"\ @@ -73,10 +74,8 @@ class Cmd def parse(args) options = { 'pose' => 0, - 'link' => 0, - 'link_name' => '', - 'joint' => 0, - 'joint_name' => '' + 'link_name' => 0, + 'joint_name' => 0 } usage = COMMANDS[args[0]] @@ -97,12 +96,17 @@ class Cmd options['pose'] = 1 end opts.on('-l', '--link [arg]', String, 'Request link information') do |l| - options['link'] = 1 - options['link_name'] = l + options['link_name'] = '' + if l + options['link_name'] = l + end end opts.on('-j', '--joint [arg]', String, 'Request joint information') do |j| - options['joint'] = 1 - options['joint_name'] = j + # options['joint'] = 1 + options['joint_name'] = '' + if j + options['joint_name'] = j + end end end # opt_parser do begin @@ -159,11 +163,10 @@ class Cmd Importer.cmdModelList() exit(0) elsif options.key?('model') - Importer.extern 'void cmdModelInfo(const char *, int, int, const char *, - int, const char *)' - - Importer.cmdModelInfo(options['model'], options['pose'], options['link'], - options['link_name'], options['joint'], options['joint_name']) + Importer.extern 'void cmdModelInfo(const char *, int, const char *, + const char *)' + Importer.cmdModelInfo(options['model'], options['pose'], + options['link_name'], options['joint_name']) else puts 'Command error: I do not have an implementation for '\ "command [ign #{options['command']}]." diff --git a/tutorials/model_command.md b/tutorials/model_command.md index dbffe2521ff..e18689a6070 100644 --- a/tutorials/model_command.md +++ b/tutorials/model_command.md @@ -1,30 +1,13 @@ \page model_command Model Command ## Overview -`ign model` command allows you to get information about the models for a given running `ignition` simulation. +`ign model` command allows you to get information about the models for a given running Ignition Gazebo simulation. For each model, it is possible to get information about its - Pose: Pose of the model - Links: Pose, mass, and inertial matrix of the link - Joints: Parent link, child link and joint type. -### Available options for the model command - -``` - --list Get a list of the available models. - -m [--model] arg Select the model to be shown. - -l [--link] arg Select a link to show its properties. - If no arg is passed all links are printed - Requires the -m option - - -j [--joint] arg Select a joint to show its properties. - If no arg is passed all joints are printed - Requires the -m option - - -h [ --help ] Print the help message. -``` - - ## Example running the diff_drive world To try out this command we need first a running simulation. Let's load the diff_drive ignition simulation. In a terminal, run: @@ -51,75 +34,98 @@ Once you get the name of the model you want to see, you may run the following co ign model -m vehicle_blue ``` - Requesting state for world [diff_drive]... + Requesting state for world [diff_drive]... + + Model: [8] + - Name: vehicle_blue + - Pose [ XYZ (m) ] [ RPY (rad) ]: + [0.000000 | 2.000000 | 0.325000] + [0.000000 | 0.000000 | 0.000000] + + - Link [9] + - Name: chassis + - Parent: vehicle_blue [8] + - Mass (kg): [1.143950] + - Inertial Pose: + [0.000000 | 0.000000 | 0.000000] + - Inertial Matrix (kg⋅m^2): + [0.126164 | 0.000000 | 0.000000] + [0.000000 | 0.416519 | 0.000000] + [0.000000 | 0.000000 | 0.481014] + - Pose [ XYZ (m) ] [ RPY (rad) ]: + [-0.151427 | 0.000000 | 0.175000] + [0.000000 | 0.000000 | 0.000000] + - Link [12] + - Name: left_wheel + - Parent: vehicle_blue [8] + - Mass (kg): [2.000000] + - Inertial Pose: + [0.000000 | 0.000000 | 0.000000] + - Inertial Matrix (kg⋅m^2): + [0.145833 | 0.000000 | 0.000000] + [0.000000 | 0.145833 | 0.000000] + [0.000000 | 0.000000 | 0.125000] + - Pose [ XYZ (m) ] [ RPY (rad) ]: + [0.554283 | 0.625029 | -0.025000] + [-1.570700 | 0.000000 | 0.000000] + - Link [15] + - Name: right_wheel + - Parent: vehicle_blue [8] + - Mass (kg): [2.000000] + - Inertial Pose: + [0.000000 | 0.000000 | 0.000000] + - Inertial Matrix (kg⋅m^2): + [0.145833 | 0.000000 | 0.000000] + [0.000000 | 0.145833 | 0.000000] + [0.000000 | 0.000000 | 0.125000] + - Pose [ XYZ (m) ] [ RPY (rad) ]: + [0.554282 | -0.625029 | -0.025000] + [-1.570700 | 0.000000 | 0.000000] + - Link [18] + - Name: caster + - Parent: vehicle_blue [8] + - Mass (kg): [1.000000] + - Inertial Pose: + [0.000000 | 0.000000 | 0.000000] + - Inertial Matrix (kg⋅m^2): + [0.100000 | 0.000000 | 0.000000] + [0.000000 | 0.100000 | 0.000000] + [0.000000 | 0.000000 | 0.100000] + - Pose [ XYZ (m) ] [ RPY (rad) ]: + [-0.957138 | 0.000000 | -0.125000] + [0.000000 | 0.000000 | 0.000000] + - Joint [21] + - Name: left_wheel_joint + - Parent: vehicle_blue [8] + - Type: revolute + - Parent Link: left_wheel + - Child Link: chassis + - Pose [ XYZ (m) ] [ RPY (rad) ]: + [0.000000 | 0.000000 | 0.000000] + [0.000000 | 0.000000 | 0.000000] + - Axis position [ XYZ ]: + [0 | 0 | 1] + - Joint [22] + - Name: right_wheel_joint + - Parent: vehicle_blue [8] + - Type: revolute + - Parent Link: right_wheel + - Child Link: chassis + - Pose [ XYZ (m) ] [ RPY (rad) ]: + [0.000000 | 0.000000 | 0.000000] + [0.000000 | 0.000000 | 0.000000] + - Axis position [ XYZ ]: + [0 | 0 | 1] + - Joint [23] + - Name: caster_wheel + - Parent: vehicle_blue [8] + - Type: ball + - Parent Link: caster + - Child Link: chassis + - Pose [ XYZ (m) ] [ RPY (rad) ]: + [0.000000 | 0.000000 | 0.000000] + [0.000000 | 0.000000 | 0.000000] - Name: vehicle_blue - - Pose: - [0.000000 | 2.000000 | 0.325000] - [0.000000 | 0.000000 | 0.000000] - - - Link [9] - - Name: chassis - - Parent: vehicle_blue [8] - - Mass: [1.143950] - - Inertial Matrix: - [0.126164 | 0.000000 | 0.000000] - [0.000000 | 0.416519 | 0.000000] - [0.000000 | 0.000000 | 0.481014] - - Pose: - [-0.151427 | 0.000000 | 0.175000] - [0.000000 | 0.000000 | 0.000000] - - Link [12] - - Name: left_wheel - - Parent: vehicle_blue [8] - - Mass: [2.000000] - - Inertial Matrix: - [0.145833 | 0.000000 | 0.000000] - [0.000000 | 0.145833 | 0.000000] - [0.000000 | 0.000000 | 0.125000] - - Pose: - [0.554283 | 0.625029 | -0.025000] - [-1.570700 | 0.000000 | 0.000000] - - Link [15] - - Name: right_wheel - - Parent: vehicle_blue [8] - - Mass: [2.000000] - - Inertial Matrix: - [0.145833 | 0.000000 | 0.000000] - [0.000000 | 0.145833 | 0.000000] - [0.000000 | 0.000000 | 0.125000] - - Pose: - [0.554282 | -0.625029 | -0.025000] - [-1.570700 | 0.000000 | 0.000000] - - Link [18] - - Name: caster - - Parent: vehicle_blue [8] - - Mass: [1.000000] - - Inertial Matrix: - [0.100000 | 0.000000 | 0.000000] - [0.000000 | 0.100000 | 0.000000] - [0.000000 | 0.000000 | 0.100000] - - Pose: - [-0.957138 | 0.000000 | -0.125000] - [0.000000 | 0.000000 | 0.000000] - - Joint [21] - - Name: left_wheel_joint - - Parent: vehicle_blue [8] - - Joint type: revolute - - Parent Link: [left_wheel] - - Child Link: [chassis] - - Joint [22] - - Name: right_wheel_joint - - Parent: vehicle_blue [8] - - Joint type: revolute - - Parent Link: [right_wheel] - - Child Link: [chassis] - - Joint [23] - - Name: caster_wheel - - Parent: vehicle_blue [8] - - Joint type: ball - - Parent Link: [caster] - - Child Link: [chassis] ``` @@ -131,8 +137,9 @@ Once you get the name of the model you want to see, you may run the following co ``` Requesting state for world [diff_drive]... - Name: vehicle_blue - - Pose: + Model: [8] + - Name: vehicle_blue + - Pose [ XYZ (m) ] [ RPY (rad) ]: [0.000000 | 2.000000 | 0.325000] [0.000000 | 0.000000 | 0.000000] ``` @@ -150,17 +157,19 @@ Or you can get the information of a **single link** by adding the name as argume ``` Requesting state for world [diff_drive]... - - Link [18] - - Name: caster - - Parent: vehicle_blue [8] - - Mass: [1.000000] - - Inertial Matrix: - [0.100000 | 0.000000 | 0.000000] - [0.000000 | 0.100000 | 0.000000] - [0.000000 | 0.000000 | 0.100000] - - Pose: - [-0.957138 | 0.000000 | -0.125000] - [0.000000 | 0.000000 | 0.000000] + - Link [18] + - Name: caster + - Parent: vehicle_blue [8] + - Mass (kg): [1.000000] + - Inertial Pose: + [0.000000 | 0.000000 | 0.000000] + - Inertial Matrix (kg⋅m^2): + [0.100000 | 0.000000 | 0.000000] + [0.000000 | 0.100000 | 0.000000] + [0.000000 | 0.000000 | 0.100000] + - Pose [ XYZ (m) ] [ RPY (rad) ]: + [-0.957138 | 0.000000 | -0.125000] + [0.000000 | -0.000000 | 0.000000] ``` @@ -174,11 +183,14 @@ Or you can get the information of a **single joint** by adding the name as argum ``` Requesting state for world [diff_drive]... - - - Joint [23] - - Name: caster_wheel - - Parent: vehicle_blue [8] - - Joint type: ball - - Parent Link: [caster] - - Child Link: [chassis] + + - Joint [23] + - Name: caster_wheel + - Parent: vehicle_blue [8] + - Type: ball + - Parent Link: caster + - Child Link: chassis + - Pose [ XYZ (m) ] [ RPY (rad) ]: + [0.000000 | 0.000000 | 0.000000] + [0.000000 | -0.000000 | 0.000000] ```