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

22
collada_parser/src/collada_parser.cpp
View File

@ -805,14 +805,7 @@ protected:
plink->visual->material->color.g = 1.0;
plink->visual->material->color.b = 0.0;
plink->visual->material->color.a = 1.0;
plink->inertial.reset(new Inertial());
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;
plink->inertial.reset();
_model->links_.insert(std::make_pair(linkname,plink));
}
@ -836,15 +829,24 @@ protected:
if( !!rigidbody && !!rigidbody->getTechnique_common() ) {
domRigid_body::domTechnique_commonRef rigiddata = rigidbody->getTechnique_common();
if( !!rigiddata->getMass() ) {
if ( !plink->inertial ) {
plink->inertial.reset(new Inertial());
}
plink->inertial->mass = rigiddata->getMass()->getValue();
}
if( !!rigiddata->getInertia() ) {
if ( !plink->inertial ) {
plink->inertial.reset(new Inertial());
}
plink->inertial->ixx = rigiddata->getInertia()->getValue()[0];
plink->inertial->iyy = rigiddata->getInertia()->getValue()[1];
plink->inertial->izz = rigiddata->getInertia()->getValue()[2];
}
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);
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;
pchildlink.reset();
++numjoints;