From 1e3f1be30a7f4fafab3d92f003e66eb0e42355ab Mon Sep 17 00:00:00 2001 From: YoheiKakiuchi Date: Tue, 4 Feb 2014 23:49:12 +0900 Subject: [PATCH] fix, joint axis should be rotated depend on local coords --- collada_parser/src/collada_parser.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/collada_parser/src/collada_parser.cpp b/collada_parser/src/collada_parser.cpp index 8e404b0..1988851 100644 --- a/collada_parser/src/collada_parser.cpp +++ b/collada_parser/src/collada_parser.cpp @@ -1022,9 +1022,18 @@ protected: pjoint->child_link_name = pchildlink->name; // Axes and Anchor assignment. - pjoint->axis.x = pdomaxis->getAxis()->getValue()[0]; - pjoint->axis.y = pdomaxis->getAxis()->getValue()[1]; - pjoint->axis.z = pdomaxis->getAxis()->getValue()[2]; + { + Vector3 ax(pdomaxis->getAxis()->getValue()[0], + pdomaxis->getAxis()->getValue()[1], + pdomaxis->getAxis()->getValue()[2]); + + // rotate axis + ax = tatt.rotation * ax; + + pjoint->axis.x = ax.x; + pjoint->axis.y = ax.y; + pjoint->axis.z = ax.z; + } if (!motion_axis_info) { ROS_WARN_STREAM(str(boost::format("No motion axis info for joint %s\n")%pjoint->name));