From 2de97f0ee39190455df250a9f5772f75b1f01bd5 Mon Sep 17 00:00:00 2001 From: YoheiKakiuchi Date: Tue, 2 Apr 2013 00:45:51 +0900 Subject: [PATCH] fix wrong inertia coordinates, added the patch from https://code.ros.org/trac/ros-pkg/ticket/5609 --- collada_parser/src/collada_parser.cpp | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/collada_parser/src/collada_parser.cpp b/collada_parser/src/collada_parser.cpp index f1a5bdc..51a2854 100644 --- a/collada_parser/src/collada_parser.cpp +++ b/collada_parser/src/collada_parser.cpp @@ -681,7 +681,12 @@ protected: ROS_DEBUG_STREAM(str(boost::format("Number of root links in the kmodel %d\n")%ktec->getLink_array().getCount())); for (size_t ilink = 0; ilink < ktec->getLink_array().getCount(); ++ilink) { - _ExtractLink(ktec->getLink_array()[ilink], ilink == 0 ? pnode : domNodeRef(), Pose(), Pose(), vdomjoints, bindings); + domLinkRef pdomlink = ktec->getLink_array()[ilink]; + _RootOrigin = _poseFromMatrix(_ExtractFullTransform(pdomlink)); + ROS_DEBUG("RootOrigin: %lf %lf %lf %lf %lf %lf %lf", + _RootOrigin.position.x, _RootOrigin.position.y, _RootOrigin.position.z, + _RootOrigin.rotation.x, _RootOrigin.rotation.y, _RootOrigin.rotation.z, _RootOrigin.rotation.w); + _ExtractLink(pdomlink, ilink == 0 ? pnode : domNodeRef(), Pose(), Pose(), vdomjoints, bindings); } // TODO: implement mathml @@ -849,7 +854,11 @@ protected: if ( !plink->inertial ) { plink->inertial.reset(new Inertial()); } - plink->inertial->origin = _poseMult(_poseInverse(tParentWorldLink), _poseFromMatrix(_ExtractFullTransform(rigiddata->getMass_frame()))); + //plink->inertial->origin = _poseMult(_poseInverse(tParentWorldLink), _poseFromMatrix(_ExtractFullTransform(rigiddata->getMass_frame()))); + Pose tlink = _poseFromMatrix(_ExtractFullTransform(pdomlink)); + plink->inertial->origin = _poseMult(_poseInverse(_poseMult(_poseInverse(_RootOrigin), + _poseMult(tParentWorldLink, tlink))), + _poseFromMatrix(_ExtractFullTransform(rigiddata->getMass_frame()))); } } @@ -862,6 +871,10 @@ protected: else { ROS_DEBUG_STREAM(str(boost::format("Attachment link elements: %d\n")%pdomlink->getAttachment_full_array().getCount())); Pose tlink = _poseFromMatrix(_ExtractFullTransform(pdomlink)); + ROS_DEBUG("tlink: %s: %lf %lf %lf %lf %lf %lf %lf", + linkname.c_str(), + tlink.position.x, tlink.position.y, tlink.position.z, + tlink.rotation.x, tlink.rotation.y, tlink.rotation.z, tlink.rotation.w); plink->visual->origin = _poseMult(tParentLink, tlink); // use the kinematics coordinate system for each link // ROS_INFO("link %s rot: %f %f %f %f",linkname.c_str(),plink->visual->origin.rotation.w, plink->visual->origin.rotation.x,plink->visual->origin.rotation.y,plink->visual->origin.rotation.z); // ROS_INFO("link %s trans: %f %f %f",linkname.c_str(),plink->visual->origin.position.x,plink->visual->origin.position.y,plink->visual->origin.position.z); @@ -2679,7 +2692,7 @@ protected: std::string _filename; std::string _resourcedir; boost::shared_ptr _model; - + Pose _RootOrigin; };