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

Add meshes handling and other improvements #46

Merged
merged 4 commits into from
Jun 15, 2017
Merged

Add meshes handling and other improvements #46

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
29d97d4
Fix compilation of the legacy generator with recent versions of urdfd…
traversaro May 8, 2017
f777127
Add r_wrist_prosup to the axes to reverse
traversaro May 8, 2017
47ce5a5
Add simmechanics file generated on Feb 2017
traversaro May 8, 2017
7b4ebcb
Add simmechanics-generated meshes
traversaro May 8, 2017
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
2 changes: 1 addition & 1 deletion dh/generator/icub_urdf_sdf_from_dh_generator.cpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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