diff --git a/tesseract_kinematics/kdl/src/kdl_utils.cpp b/tesseract_kinematics/kdl/src/kdl_utils.cpp index 31d4df53a69..ff4a47dc22f 100644 --- a/tesseract_kinematics/kdl/src/kdl_utils.cpp +++ b/tesseract_kinematics/kdl/src/kdl_utils.cpp @@ -43,8 +43,6 @@ void KDLToEigen(const KDL::Frame& frame, Eigen::Isometry3d& transform) void EigenToKDL(const Eigen::Isometry3d& transform, KDL::Frame& frame) { - frame.Identity(); - for (int i = 0; i < 3; ++i) frame.p[i] = transform(i, 3);