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

Merge pull request #92 from YoheiKakiuchi/fix_jointaxis_rotation-indigo-devel 

Fix rotation of joint axis
This commit is contained in:
Ioan A Sucan 2014-11-25 08:19:25 -08:00
parent 60dac130dd f633960a38
commit f07a95bde1
1 changed files with 17 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
collada_parser/src/collada_parser.cpp
View File

@ -1038,18 +1038,32 @@ protected:
boost::shared_ptr<Joint> pjoint = vjoints[ic];
pjoint->child_link_name = pchildlink->name;
// Axes and Anchor assignment.
#define PRINT_POSE(pname, apose) ROS_DEBUG(pname" pos: %f %f %f, rot: %f %f %f %f", \
apose.position.x, apose.position.y, apose.position.z, \
apose.rotation.x, apose.rotation.y, apose.rotation.z, apose.rotation.w);
{
PRINT_POSE("tatt", tatt);
PRINT_POSE("trans_joint_to_child", tatt);
Pose trans_joint_to_child = _poseFromMatrix(_ExtractFullTransform(pattfull->getLink()));
pjoint->parent_to_joint_origin_transform = _poseMult(tatt, trans_joint_to_child);
// Axes and Anchor assignment.
Vector3 ax(pdomaxis->getAxis()->getValue()[0],
pdomaxis->getAxis()->getValue()[1],
pdomaxis->getAxis()->getValue()[2]);
Pose pinv = _poseInverse(trans_joint_to_child);
// rotate axis
ax = tatt.rotation * ax;
ax = pinv.rotation * ax;
pjoint->axis.x = ax.x;
pjoint->axis.y = ax.y;
pjoint->axis.z = ax.z;
ROS_DEBUG("joint %s axis: %f %f %f -> %f %f %f\n", pjoint->name.c_str(),
pdomaxis->getAxis()->getValue()[0],
pdomaxis->getAxis()->getValue()[1],
pdomaxis->getAxis()->getValue()[2],
pjoint->axis.x, pjoint->axis.y, pjoint->axis.z);
}
if (!motion_axis_info) {
@ -1122,8 +1136,6 @@ 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 = _poseMult(tatt,_poseFromMatrix(_ExtractFullTransform(pattfull->getLink())));
if (pjoint->limits->velocity == 0.0) {
pjoint->limits->velocity = pjoint->type == Joint::PRISMATIC ? 0.01 : 0.5f;
}