Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix rbdl model #81

Merged
merged 2 commits into from
Oct 25, 2023
Merged

Fix rbdl model #81

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
bbd402e
Remove deprecated header include
dmronga Oct 25, 2023
9cd013c
Add a temp. workaround to RBDL model: RBDL does not support rotations
dmronga Oct 25, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 0 additions & 1 deletion src/robot_models/pinocchio/RobotModelPinocchio.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
#ifndef ROBOT_MODEL_PINOCCHIO_HPP
#define ROBOT_MODEL_PINOCCHIO_HPP

#include "../../core/RobotModelFactory.hpp"
#include "../../core/RobotModel.hpp"
#include <pinocchio/multibody/fwd.hpp>
#include <pinocchio/parsers/urdf.hpp>
Expand Down
26 changes: 19 additions & 7 deletions src/robot_models/rbdl/RobotModelRBDL.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,25 +41,37 @@ bool RobotModelRBDL::configure(const RobotModelConfig& cfg){
}
base_frame = robot_urdf->getRoot()->name;

// Add floating base
joint_names = URDFTools::jointNamesFromURDF(robot_urdf);

// Temporary workaround: RBDL does not support rotations of the link invertias. Set them to zero.
// Note: This will lead to inconsistent behavior in acceleration-based WBC
for(auto &l : robot_urdf->links_){
if(l.second->inertial)
l.second->inertial->origin.rotation.setFromRPY(0,0,0);
}
TiXmlDocument *doc = urdf::exportURDF(robot_urdf);
std::string robot_urdf_file = "/tmp/robot.urdf";
doc->SaveFile(robot_urdf_file);

if(!Addons::URDFReadFromFile(robot_urdf_file.c_str(), rbdl_model.get(), cfg.floating_base)){
LOG_ERROR_S << "Unable to parse urdf from file " << cfg.file << std::endl;
return false;
}

// Add floating base to robot_urdf. We will not load this URDF model in RBDL, since RBDL adds its own floating base.
// However, we need the robot_urdf for some internal functionalities.
has_floating_base = cfg.floating_base;
world_frame = base_frame;
if(cfg.floating_base){
joint_names_floating_base = URDFTools::addFloatingBaseToURDF(robot_urdf);
world_frame = robot_urdf->getRoot()->name;
}

if(!Addons::URDFReadFromFile(cfg.file.c_str(), rbdl_model.get(), cfg.floating_base)){
LOG_ERROR_S << "Unable to parse urdf from file " << cfg.file << std::endl;
return false;
}

actuated_joint_names = joint_names;
joint_names = independent_joint_names = joint_names_floating_base + joint_names;

// Read Joint Limits
URDFTools::jointLimitsFromURDF(robot_urdf, joint_limits);
URDFTools::jointLimitsFromURDF(robot_urdf, joint_limits);

// 2. Verify consistency of URDF and config

Expand Down
Loading