tmp

Author: HiroIshida

src/kinematics.cpp

@@ -84,9 +84,10 @@ void KinematicModel::get_link_pose_inner(
       } else if (pjoint->type == urdf::Joint::REVOLUTE ||
                  pjoint->type == urdf::Joint::CONTINUOUS) {
         double angle = joint_angles_[pjoint->id];
-        Transform tf_pjoint_to_hlink = pjoint->transform(angle);
-        tf_plink_to_hlink = tf_plink_to_pjoint *
-                            urdf_pose_to_eigen_affine3d(tf_pjoint_to_hlink);
+        Eigen::AngleAxisd tf_pjoint_to_hlink{
+            angle,
+            Eigen::Vector3d{pjoint->axis.x, pjoint->axis.y, pjoint->axis.z}};
+        tf_plink_to_hlink = tf_plink_to_pjoint * tf_pjoint_to_hlink;
       } else {
         assert(false && "unknown joint type");
       }