bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

collada parser fixes with inertial frames and parent_to_joint_origin_transform

This commit is contained in:
Kei Okada 2012-07-06 01:13:28 +09:00
parent fb906d5fb9
commit faa9b022d2
1 changed files with 16 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
collada_parser/src/collada_parser.cpp
View File

@ -502,21 +502,21 @@ protected:
// building tree: name mapping // building tree: name mapping
try try
{ {
_model->initTree(parent_link_tree); _model->initTree(parent_link_tree);
} }
catch(ParseError &e) catch(ParseError &e)
{ {
ROS_ERROR("Failed to build tree: %s", e.what()); ROS_ERROR("Failed to build tree: %s", e.what());
} }
// find the root link // find the root link
try try
{ {
_model->initRoot(parent_link_tree); _model->initRoot(parent_link_tree);
} }
catch(ParseError &e) catch(ParseError &e)
{ {
ROS_ERROR("Failed to find root link: %s", e.what()); ROS_ERROR("Failed to find root link: %s", e.what());
} }
} }
@ -805,14 +805,7 @@ protected:
plink->visual->material->color.g = 1.0; plink->visual->material->color.g = 1.0;
plink->visual->material->color.b = 0.0; plink->visual->material->color.b = 0.0;
plink->visual->material->color.a = 1.0; plink->visual->material->color.a = 1.0;
plink->inertial.reset(new Inertial()); plink->inertial.reset();
plink->inertial->mass = 1.0;
plink->inertial->ixx = 1.0;
plink->inertial->iyy = 1.0;
plink->inertial->izz = 1.0;
plink->inertial->ixy = 0.0;
plink->inertial->ixz = 0.0;
plink->inertial->iyz = 0.0;
_model->links_.insert(std::make_pair(linkname,plink)); _model->links_.insert(std::make_pair(linkname,plink));
} }
@ -836,15 +829,24 @@ protected:
if( !!rigidbody && !!rigidbody->getTechnique_common() ) { if( !!rigidbody && !!rigidbody->getTechnique_common() ) {
domRigid_body::domTechnique_commonRef rigiddata = rigidbody->getTechnique_common(); domRigid_body::domTechnique_commonRef rigiddata = rigidbody->getTechnique_common();
if( !!rigiddata->getMass() ) { if( !!rigiddata->getMass() ) {
if ( !plink->inertial ) {
plink->inertial.reset(new Inertial());
}
plink->inertial->mass = rigiddata->getMass()->getValue(); plink->inertial->mass = rigiddata->getMass()->getValue();
} }
if( !!rigiddata->getInertia() ) { if( !!rigiddata->getInertia() ) {
if ( !plink->inertial ) {
plink->inertial.reset(new Inertial());
}
plink->inertial->ixx = rigiddata->getInertia()->getValue()[0]; plink->inertial->ixx = rigiddata->getInertia()->getValue()[0];
plink->inertial->iyy = rigiddata->getInertia()->getValue()[1]; plink->inertial->iyy = rigiddata->getInertia()->getValue()[1];
plink->inertial->izz = rigiddata->getInertia()->getValue()[2]; plink->inertial->izz = rigiddata->getInertia()->getValue()[2];
} }
if( !!rigiddata->getMass_frame() ) { if( !!rigiddata->getMass_frame() ) {
plink->inertial->origin = _poseMult(tParentLink, _poseFromMatrix(_ExtractFullTransform(rigiddata->getMass_frame()))); if ( !plink->inertial ) {
plink->inertial.reset(new Inertial());
}
plink->inertial->origin = _poseMult(_poseInverse(tParentWorldLink), _poseFromMatrix(_ExtractFullTransform(rigiddata->getMass_frame())));
} }
} }
@ -1077,7 +1079,7 @@ protected:
} }
//ROS_INFO("joint %s axis: %f %f %f",pjoint->name.c_str(),pjoint->axis.x,pjoint->axis.y,pjoint->axis.z); //ROS_INFO("joint %s axis: %f %f %f",pjoint->name.c_str(),pjoint->axis.x,pjoint->axis.y,pjoint->axis.z);
pjoint->parent_to_joint_origin_transform = tatt; pjoint->parent_to_joint_origin_transform = _poseMult(tatt,_poseFromMatrix(_ExtractFullTransform(pattfull->getLink())));
pjoint->limits->velocity = pjoint->type == Joint::PRISMATIC ? 0.01 : 0.5f; pjoint->limits->velocity = pjoint->type == Joint::PRISMATIC ? 0.01 : 0.5f;
pchildlink.reset(); pchildlink.reset();
++numjoints; ++numjoints;