Skip to content

Commit

Permalink
Fix compilation of the legacy generator with recent versions of urdfd…
Browse files Browse the repository at this point in the history 
…om and sdformat
  • Loading branch information
@traversaro
traversaro committed May 8, 2017
1 parent 6212634 commit 29d97d4
Show file tree
Hide file tree
Showing 5 changed files with 63 additions and 63 deletions.
2 changes: 1 addition & 1 deletion dh/generator/icub_urdf_sdf_from_dh_generator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,7 @@ int main(int argc, char* argv[])
//Generating model for black iCub
// robot_name directory head legs feet Paris02 gazeboSim
bool simple_meshes = false;
boost::shared_ptr<urdf::ModelInterface> model;
urdf::ModelInterfaceSharedPtr model;
bool ok = generate_iCub_urdf_model(opt.find("YARP_ROBOT_NAME").asString(),
version_head , version_legs , version_feet,
isiCubParis02, false, simple_meshes ,
Expand Down
21 changes: 11 additions & 10 deletions dh/generator/urdf_sdf_from_dh_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -333,6 +333,7 @@ bool addGazeboSurfaceFrictionInformationToCollisionSDF(sdf::ElementPtr collision
AddElementAndSetValue(bounce,"restitution_coefficient","double","0.000000");
AddElementAndSetValue(bounce,"threshold","double","100000.000000");

return true;
}


Expand Down Expand Up @@ -537,6 +538,8 @@ bool substituteCollisionWithBoxInSDF(sdf::SDFPtr icub_sdf,
}
}
}

return true;
}

bool generate_model_config_file(std::string robot_name, std::string gazebo_robot_model_directory)
Expand Down Expand Up @@ -569,7 +572,7 @@ bool generate_iCub_urdf_model(std::string iCub_name,
double mass_epsilon,
double inertia_epsilon,
bool noFTsimulation,
boost::shared_ptr<urdf::ModelInterface> urdf_file,
urdf::ModelInterfaceSharedPtr urdf_file,
std::string outputfilename
)
{
Expand Down Expand Up @@ -614,7 +617,7 @@ bool generate_iCub_urdf_model(std::string iCub_name,
return false;
}

boost::shared_ptr<urdf::ModelInterface> urdf_idyn(new urdf::ModelInterface);
urdf::ModelInterfaceSharedPtr urdf_idyn(new urdf::ModelInterface);

//std::cout << "iCub KDL::Tree: " << std::endl;
//std::cout << icub_kdl << std::endl;
Expand All @@ -628,7 +631,7 @@ bool generate_iCub_urdf_model(std::string iCub_name,
//////////////////////////////////////////////////////////////////
//// Getting meshes and limits from urdf paris files
//////////////////////////////////////////////////////////////////
boost::shared_ptr<urdf::ModelInterface> urdf_paris;
urdf::ModelInterfaceSharedPtr urdf_paris;

std::string filename_urdf_paris = paris_directory+paris_subdirectory+"/icub.xml";

Expand Down Expand Up @@ -716,7 +719,7 @@ bool generate_iCub_sdf_model(std::string iCub_name,
double mass_epsilon,
double inertia_epsilon,
bool noFTsimulation,
boost::shared_ptr<urdf::ModelInterface> urdf_idyn,
urdf::ModelInterfaceSharedPtr urdf_idyn,
std::string outputfilename
)
{
Expand Down Expand Up @@ -770,18 +773,14 @@ bool generate_iCub_sdf_model(std::string iCub_name,
//std::string gazebo_conversion_command = "gzsdf print " + filename_urdf_gazebo_conversion + " > " + gazebo_sdf_filename;
//std::cout << "Running command: " << gazebo_conversion_command << std::endl;

sdf::URDF2SDF urdf_converter;
TiXmlDocument sdf_xml = urdf_converter.InitModelString(urdf_for_gazebo_conversion_string);

//system(gazebo_conversion_command.c_str());
//if( !system(gazebo_conversion_command.c_str()) ) { std::cerr << "Error in urdf - sdf conversion" << std::endl; return false; }

//\todo: adding plugins to generated sdf
sdf::SDFPtr icub_sdf(new sdf::SDF());
sdf::init(icub_sdf);

//if( ! sdf::readFile(gazebo_sdf_filename,icub_sdf) ) { std::cerr << "Problem in reading SDF file" << std::endl; return false; }
if( ! sdf::readDoc(&sdf_xml,icub_sdf,"custom sdf xml") ) { std::cerr << "Problem in loading SDF file" << std::endl; return false; }
if( ! sdf::readString(urdf_for_gazebo_conversion_string,icub_sdf) ) { std::cerr << "Problem in reading SDF file" << std::endl; return false; }

if( ! icub_sdf->Root()->HasElement("model") ) { std::cerr << "Problem in parsing SDF dom" << std::endl; return false; }

Expand Down Expand Up @@ -867,7 +866,7 @@ bool generate_iCub_model(std::string iCub_name,
bool noFTsimulation)
{
bool ok = true;
boost::shared_ptr<urdf::ModelInterface> urdf_file;
urdf::ModelInterfaceSharedPtr urdf_file;
ok = ok && generate_iCub_urdf_model(iCub_name,head_version,legs_version,feet_version,is_iCubParis02,is_icubGazeboSim,simple_meshes,data_directory,mass_epsilon,inertia_epsilon,noFTsimulation,urdf_file,root_directory+"/icub.urdf");
ok = ok && generate_iCub_sdf_model(iCub_name,head_version,legs_version,feet_version,is_iCubParis02,is_icubGazeboSim,simple_meshes,data_directory,mass_epsilon,inertia_epsilon,noFTsimulation,urdf_file);
return true;
Expand Down Expand Up @@ -895,4 +894,6 @@ bool generate_gazebo_database(const std::vector<std::string> & robot_names, cons

database_file << "</models>\n</database>\n" << std::endl;
database_file.close();

return true;
}
7 changes: 2 additions & 5 deletions dh/generator/urdf_sdf_from_dh_utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,9 +25,6 @@
#include <urdf_parser/urdf_parser.h>

#include <tinyxml.h>

#include <boost/function.hpp>

#include <yarp/os/Property.h>
#include <yarp/os/Os.h>

Expand Down Expand Up @@ -122,7 +119,7 @@ bool generate_iCub_urdf_model(std::string iCub_name,
double mass_epsilon,
double inertia_epsilon,
bool noFTsimulation,
boost::shared_ptr<urdf::ModelInterface> urdf_file,
urdf::ModelInterfaceSharedPtr urdf_file,
std::string outputfile );

bool generate_iCub_sdf_model(std::string iCub_name,
Expand All @@ -136,7 +133,7 @@ bool generate_iCub_sdf_model(std::string iCub_name,
double mass_epsilon,
double inertia_epsilon,
bool noFTsimulation,
boost::shared_ptr<urdf::ModelInterface> urdf_file);
urdf::ModelInterfaceSharedPtr urdf_file);

/**
* Generate iCub URDF and SDF models, starting from UPMC meshes and kinematics/dynamics information from iDyn
Expand Down
76 changes: 39 additions & 37 deletions dh/generator/urdf_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,8 @@

#include "urdf_utils.h"

#include <urdf_model/model.h>

#include <kdl/tree.hpp>

#include <kdl/treefksolverpos_recursive.hpp>
Expand All @@ -14,16 +16,16 @@
#include <iDynTree/ModelIO/impl/urdf_import.hpp>

///< \todo TODO add support for deleting a long chain of null link connected by fixed base
bool deleteLink(boost::shared_ptr<urdf::ModelInterface> urdf_input, std::string link_to_delete, bool verbose=false)
bool deleteLink(urdf::ModelInterfaceSharedPtr urdf_input, std::string link_to_delete, bool verbose=false)
{
if( link_to_delete != urdf_input->getRoot()->name ) {
//deleting normal link

//delete links (remove from global and from parent (also for joint)
boost::shared_ptr<urdf::Link> link_sptr = urdf_input->links_[link_to_delete];
urdf::LinkSharedPtr link_sptr = urdf_input->links_[link_to_delete];
if( link_sptr->child_links.size() != 0 ) { std::cerr << "deleteLink error: tryng to delete a link with a child" << std::endl; return false;}
boost::shared_ptr<urdf::Joint> joint_sptr = link_sptr->parent_joint;
boost::shared_ptr<urdf::Link> parent_link = link_sptr->getParent();
urdf::JointSharedPtr joint_sptr = link_sptr->parent_joint;
urdf::LinkSharedPtr parent_link = link_sptr->getParent();

//remove links from cparent_link->child of parent
for(int i=0; i < parent_link->child_links.size(); i++ ) {
Expand All @@ -46,13 +48,13 @@ bool deleteLink(boost::shared_ptr<urdf::ModelInterface> urdf_input, std::string
} else {
//deleting root link

boost::shared_ptr<urdf::Link> link_sptr = urdf_input->links_[link_to_delete];
urdf::LinkSharedPtr link_sptr = urdf_input->links_[link_to_delete];

if( link_sptr->child_links.size() != 1 ) { std::cerr << "deleteLink error: tryng to delete a root link with more than a child" << std::endl; return false; }

boost::shared_ptr<urdf::Joint> joint_sptr = link_sptr->child_joints[0];
urdf::JointSharedPtr joint_sptr = link_sptr->child_joints[0];

boost::shared_ptr<urdf::Link> new_root = link_sptr->child_links[0];
urdf::LinkSharedPtr new_root = link_sptr->child_links[0];

urdf_input->root_link_ = new_root;

Expand All @@ -65,7 +67,7 @@ bool deleteLink(boost::shared_ptr<urdf::ModelInterface> urdf_input, std::string

}

bool deleteLinks(boost::shared_ptr<urdf::ModelInterface> urdf_input, std::vector<std::string> linksToDelete, bool verbose=false)
bool deleteLinks(urdf::ModelInterfaceSharedPtr urdf_input, std::vector<std::string> linksToDelete, bool verbose=false)
{
for(int i=0; i < linksToDelete.size(); i++ ) {
if( !deleteLink(urdf_input,linksToDelete[i],verbose) ) return false;
Expand Down Expand Up @@ -116,7 +118,7 @@ KDL::Frame toKdl(urdf::Pose p)

double getTotalMass(urdf::ModelInterface & model)
{
std::vector<boost::shared_ptr<urdf::Link> > input_links;
std::vector<urdf::LinkSharedPtr > input_links;

model.getLinks(input_links);

Expand All @@ -133,8 +135,8 @@ double getTotalMass(urdf::ModelInterface & model)
return total_mass;
}

bool urdf_import_meshes(boost::shared_ptr<urdf::ModelInterface> urdf_input,
boost::shared_ptr<urdf::ModelInterface> urdf_meshes,
bool urdf_import_meshes(urdf::ModelInterfaceSharedPtr urdf_input,
urdf::ModelInterfaceSharedPtr urdf_meshes,
bool verbose)
{
bool ret = true;
Expand All @@ -159,7 +161,7 @@ bool urdf_import_meshes(boost::shared_ptr<urdf::ModelInterface> urdf_input,
KDL::TreeFkSolverPos_recursive input_solver(kdl_input);

//Iterate over the links of urdf_input, and copy the mesh
std::vector<boost::shared_ptr<urdf::Link> > input_links;
std::vector<urdf::LinkSharedPtr > input_links;

urdf_input->getLinks(input_links);

Expand All @@ -173,7 +175,7 @@ bool urdf_import_meshes(boost::shared_ptr<urdf::ModelInterface> urdf_input,

if( verbose ) std::cout << "Processing link " << input_name << std::endl;

boost::shared_ptr<const urdf::Link> mesh_link_ptr = urdf_meshes->getLink(input_name);
urdf::LinkConstSharedPtr mesh_link_ptr = urdf_meshes->getLink(input_name);


if( mesh_link_ptr ) {
Expand Down Expand Up @@ -216,9 +218,9 @@ bool urdf_import_meshes(boost::shared_ptr<urdf::ModelInterface> urdf_input,
break;
case urdf::Geometry::MESH:
input_links[i]->collision->geometry.reset(new urdf::Mesh);
if( verbose ) std::cout << "Copyng collision mesh with filename " << (boost::static_pointer_cast<urdf::Mesh>(mesh_link_ptr->collision->geometry))->filename << std::endl;
(boost::static_pointer_cast<urdf::Mesh>(input_links[i]->collision->geometry))->filename = (boost::static_pointer_cast<urdf::Mesh>(mesh_link_ptr->collision->geometry))->filename;
(boost::static_pointer_cast<urdf::Mesh>(input_links[i]->collision->geometry))->scale = (boost::static_pointer_cast<urdf::Mesh>(mesh_link_ptr->collision->geometry))->scale;
if( verbose ) std::cout << "Copyng collision mesh with filename " << (urdf::static_pointer_cast<urdf::Mesh>(mesh_link_ptr->collision->geometry))->filename << std::endl;
(urdf::static_pointer_cast<urdf::Mesh>(input_links[i]->collision->geometry))->filename = (urdf::static_pointer_cast<urdf::Mesh>(mesh_link_ptr->collision->geometry))->filename;
(urdf::static_pointer_cast<urdf::Mesh>(input_links[i]->collision->geometry))->scale = (urdf::static_pointer_cast<urdf::Mesh>(mesh_link_ptr->collision->geometry))->scale;
break;
}

Expand Down Expand Up @@ -256,9 +258,9 @@ bool urdf_import_meshes(boost::shared_ptr<urdf::ModelInterface> urdf_input,
break;
case urdf::Geometry::MESH:
input_links[i]->visual->geometry.reset(new urdf::Mesh);
if( verbose ) std::cout << "Copyng visual mesh with filename " << (boost::static_pointer_cast<urdf::Mesh>(mesh_link_ptr->visual->geometry))->filename << std::endl;
(boost::static_pointer_cast<urdf::Mesh>(input_links[i]->visual->geometry))->filename = (boost::static_pointer_cast<urdf::Mesh>(mesh_link_ptr->visual->geometry))->filename;
(boost::static_pointer_cast<urdf::Mesh>(input_links[i]->visual->geometry))->scale = (boost::static_pointer_cast<urdf::Mesh>(mesh_link_ptr->visual->geometry))->scale;
if( verbose ) std::cout << "Copyng visual mesh with filename " << (urdf::static_pointer_cast<urdf::Mesh>(mesh_link_ptr->visual->geometry))->filename << std::endl;
(urdf::static_pointer_cast<urdf::Mesh>(input_links[i]->visual->geometry))->filename = (urdf::static_pointer_cast<urdf::Mesh>(mesh_link_ptr->visual->geometry))->filename;
(urdf::static_pointer_cast<urdf::Mesh>(input_links[i]->visual->geometry))->scale = (urdf::static_pointer_cast<urdf::Mesh>(mesh_link_ptr->visual->geometry))->scale;
break;
}

Expand All @@ -273,17 +275,17 @@ bool urdf_import_meshes(boost::shared_ptr<urdf::ModelInterface> urdf_input,
return true;
}

bool urdf_import_limits(boost::shared_ptr<urdf::ModelInterface> urdf_input, boost::shared_ptr<urdf::ModelInterface> urdf_limits)
bool urdf_import_limits(urdf::ModelInterfaceSharedPtr urdf_input, urdf::ModelInterfaceSharedPtr urdf_limits)
{
//Iterate over the joints of urdf_input, and copy the limits
for( std::map<std::string, boost::shared_ptr<urdf::Joint> >::iterator it = urdf_input->joints_.begin();
for( std::map<std::string, urdf::JointSharedPtr >::iterator it = urdf_input->joints_.begin();
it != urdf_input->joints_.end(); it++ )
{
std::string joint_name = it->second->name;
std::string link_name = it->first;
std::cout << "urdf_import_limits: Processing joint " << joint_name << " relative to link " << link_name << std::endl;

boost::shared_ptr<const urdf::Joint> limits_joints_ptr = urdf_limits->getJoint(joint_name);
urdf::JointConstSharedPtr limits_joints_ptr = urdf_limits->getJoint(joint_name);


if( limits_joints_ptr ) {
Expand All @@ -302,10 +304,10 @@ bool urdf_import_limits(boost::shared_ptr<urdf::ModelInterface> urdf_input, boos
return true;
}

bool urdf_set_friction_parameters(boost::shared_ptr<urdf::ModelInterface> urdf_input, double damping, double friction)
bool urdf_set_friction_parameters(urdf::ModelInterfaceSharedPtr urdf_input, double damping, double friction)
{
//Iterate over the joints of urdf_input, and copy the limits
for( std::map<std::string, boost::shared_ptr<urdf::Joint> >::iterator it = urdf_input->joints_.begin();
for( std::map<std::string, urdf::JointSharedPtr >::iterator it = urdf_input->joints_.begin();
it != urdf_input->joints_.end(); it++ )
{
if( it->second->type == urdf::Joint::REVOLUTE ||
Expand All @@ -320,10 +322,10 @@ bool urdf_set_friction_parameters(boost::shared_ptr<urdf::ModelInterface> urdf_i
return true;
}

bool urdf_print_hom_transformations(boost::shared_ptr<urdf::ModelInterface> urdf_input)
bool urdf_print_hom_transformations(urdf::ModelInterfaceSharedPtr urdf_input)
{
//Iterate over the joints of urdf_input, and print the transformation matrix
for( std::map<std::string, boost::shared_ptr<urdf::Joint> >::iterator it = urdf_input->joints_.begin();
for( std::map<std::string, urdf::JointSharedPtr >::iterator it = urdf_input->joints_.begin();
it != urdf_input->joints_.end(); it++ )
{
std::string joint_name = it->second->name;
Expand All @@ -342,18 +344,18 @@ bool urdf_print_hom_transformations(boost::shared_ptr<urdf::ModelInterface> urdf
return true;
}

bool urdf_gazebo_cleanup_remove_massless_root(boost::shared_ptr<urdf::ModelInterface> urdf_input)
bool urdf_gazebo_cleanup_remove_massless_root(urdf::ModelInterfaceSharedPtr urdf_input)
{
if( !(urdf_input->getRoot()->inertial) ) {
if( !deleteLink(urdf_input,urdf_input->getRoot()->name) ) return EXIT_FAILURE;
}
return true;
}

bool urdf_gazebo_cleanup_remove_frames(boost::shared_ptr<urdf::ModelInterface> urdf_input)
bool urdf_gazebo_cleanup_remove_frames(urdf::ModelInterfaceSharedPtr urdf_input)
{
std::vector<std::string> linksToDelete;
std::vector<boost::shared_ptr<urdf::Link> > input_links;
std::vector<urdf::LinkSharedPtr > input_links;

input_links.clear();
urdf_input->getLinks(input_links);
Expand Down Expand Up @@ -396,9 +398,9 @@ bool hasEnding (std::string const &fullString, std::string const &ending)
}
}

bool urdf_gazebo_cleanup_transform_FT_sensors(boost::shared_ptr<urdf::ModelInterface> urdf_input)
bool urdf_gazebo_cleanup_transform_FT_sensors(urdf::ModelInterfaceSharedPtr urdf_input)
{
std::vector<boost::shared_ptr<urdf::Link> > input_links;
std::vector<urdf::LinkSharedPtr > input_links;
input_links.clear();

urdf_input->getLinks(input_links);
Expand Down Expand Up @@ -426,9 +428,9 @@ bool urdf_gazebo_cleanup_transform_FT_sensors(boost::shared_ptr<urdf::ModelInter
return true;
}

bool urdf_gazebo_cleanup_regularize_masses(boost::shared_ptr<urdf::ModelInterface> urdf_input, double mass_epsilon, double inertia_epsilon)
bool urdf_gazebo_cleanup_regularize_masses(urdf::ModelInterfaceSharedPtr urdf_input, double mass_epsilon, double inertia_epsilon)
{
std::vector<boost::shared_ptr<urdf::Link> > input_links;
std::vector<urdf::LinkSharedPtr > input_links;
input_links.clear();
urdf_input->getLinks(input_links);
for(int i=0; i < input_links.size(); i++ )
Expand Down Expand Up @@ -463,9 +465,9 @@ bool urdf_gazebo_cleanup_regularize_masses(boost::shared_ptr<urdf::ModelInterfac
}


bool urdf_gazebo_cleanup_add_model_uri(boost::shared_ptr<urdf::ModelInterface> urdf_input,std::string rule4_prefix)
bool urdf_gazebo_cleanup_add_model_uri(urdf::ModelInterfaceSharedPtr urdf_input,std::string rule4_prefix)
{
std::vector<boost::shared_ptr<urdf::Link> > input_links;
std::vector<urdf::LinkSharedPtr > input_links;
input_links.clear();
urdf_input->getLinks(input_links);

Expand All @@ -474,13 +476,13 @@ bool urdf_gazebo_cleanup_add_model_uri(boost::shared_ptr<urdf::ModelInterface> u
//Rule 4
for(int j=0; j < input_links[i]->visual_array.size(); j++ ) {
if( input_links[i]->visual_array[j]->geometry->type == urdf::Geometry::MESH ) {
(boost::static_pointer_cast<urdf::Mesh>(input_links[i]->visual_array[j]->geometry))->filename = rule4_prefix + (boost::static_pointer_cast<urdf::Mesh>(input_links[i]->visual_array[j]->geometry))->filename;
(urdf::static_pointer_cast<urdf::Mesh>(input_links[i]->visual_array[j]->geometry))->filename = rule4_prefix + (urdf::static_pointer_cast<urdf::Mesh>(input_links[i]->visual_array[j]->geometry))->filename;
}
}

for(int j=0; j < input_links[i]->collision_array.size(); j++ ) {
if( input_links[i]->collision_array[j]->geometry->type == urdf::Geometry::MESH ) {
(boost::static_pointer_cast<urdf::Mesh>(input_links[i]->collision_array[j]->geometry))->filename = rule4_prefix + (boost::static_pointer_cast<urdf::Mesh>(input_links[i]->collision_array[j]->geometry))->filename;
(urdf::static_pointer_cast<urdf::Mesh>(input_links[i]->collision_array[j]->geometry))->filename = rule4_prefix + (urdf::static_pointer_cast<urdf::Mesh>(input_links[i]->collision_array[j]->geometry))->filename;
}
}
std::cout << "urdf_gazebo_cleanup: Rule 4 applied successfully with prefix " << rule4_prefix << " on link " << input_links[i]->name << " ( i : " << i << " ) " << std::endl;
Expand Down
Loading

0 comments on commit 29d97d4

Please sign in to comment.