From df75c567d79a97ee2a66513bb249deef32fe8d65 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 1 Jan 2019 09:02:02 +0100 Subject: [PATCH] Eigen::Affine -> Eigen::Isometry --- include/bio_ik/frame.h | 2 +- src/forward_kinematics.h | 2 +- src/kinematics_plugin.cpp | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/bio_ik/frame.h b/include/bio_ik/frame.h index eadafc4..2e69690 100644 --- a/include/bio_ik/frame.h +++ b/include/bio_ik/frame.h @@ -68,7 +68,7 @@ struct Frame tf::quaternionMsgToTF(msg.orientation, rot); pos = tf::Vector3(msg.position.x, msg.position.y, msg.position.z); } - explicit inline Frame(const Eigen::Affine3d& f) + explicit inline Frame(const Eigen::Isometry3d& f) { pos = tf::Vector3(f.translation().x(), f.translation().y(), f.translation().z()); Eigen::Quaterniond q(f.rotation()); diff --git a/src/forward_kinematics.h b/src/forward_kinematics.h index 904a1f7..97e3321 100644 --- a/src/forward_kinematics.h +++ b/src/forward_kinematics.h @@ -128,7 +128,7 @@ class RobotJointEvaluator default: { auto* joint_variables = variables + joint_model->getFirstVariableIndex(); - Eigen::Affine3d joint_transform; + Eigen::Isometry3d joint_transform; joint_model->computeTransform(joint_variables, joint_transform); frame = Frame(joint_transform); break; diff --git a/src/kinematics_plugin.cpp b/src/kinematics_plugin.cpp index 0f5049b..0bedc19 100644 --- a/src/kinematics_plugin.cpp +++ b/src/kinematics_plugin.cpp @@ -153,7 +153,7 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { return false; } - std::vector tip_reference_frames; + EigenSTL::vector_Isometry3d tip_reference_frames; mutable std::vector> default_goals; @@ -466,7 +466,7 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { // transform tips to baseframe tipFrames.clear(); for (size_t i = 0; i < ik_poses.size(); i++) { - Eigen::Affine3d p, r; + Eigen::Isometry3d p, r; tf::poseMsgToEigen(ik_poses[i], p); if (context_state) { r = context_state->getGlobalLinkTransform(getBaseFrame());