From 6c31039088ff7ee8667ee690dfa844791de235f0 Mon Sep 17 00:00:00 2001 From: kkufieta Date: Thu, 24 Jan 2019 12:41:27 -0500 Subject: [PATCH] Switching from Affine3d to Isometry3d following recent MoveIt changes --- descartes_tutorials/CMakeLists.txt | 1 + descartes_tutorials/src/tutorial1.cpp | 10 +++++----- descartes_tutorials/src/tutorial2.cpp | 12 ++++++------ 3 files changed, 12 insertions(+), 11 deletions(-) diff --git a/descartes_tutorials/CMakeLists.txt b/descartes_tutorials/CMakeLists.txt index 0f58327..0f3c400 100644 --- a/descartes_tutorials/CMakeLists.txt +++ b/descartes_tutorials/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS trajectory_msgs tf tf_conversions + eigen_conversions ) catkin_package() diff --git a/descartes_tutorials/src/tutorial1.cpp b/descartes_tutorials/src/tutorial1.cpp index e775bb5..83e0f24 100644 --- a/descartes_tutorials/src/tutorial1.cpp +++ b/descartes_tutorials/src/tutorial1.cpp @@ -152,7 +152,7 @@ int main(int argc, char** argv) return 0; } -descartes_core::TrajectoryPtPtr makeCartesianPoint(const Eigen::Affine3d& pose, double dt) +descartes_core::TrajectoryPtPtr makeCartesianPoint(const Eigen::Isometry3d& pose, double dt) { using namespace descartes_core; using namespace descartes_trajectory; @@ -160,7 +160,7 @@ descartes_core::TrajectoryPtPtr makeCartesianPoint(const Eigen::Affine3d& pose, return TrajectoryPtPtr( new CartTrajectoryPt( TolerancedFrame(pose), TimingConstraint(dt)) ); } -descartes_core::TrajectoryPtPtr makeTolerancedCartesianPoint(const Eigen::Affine3d& pose, double dt) +descartes_core::TrajectoryPtPtr makeTolerancedCartesianPoint(const Eigen::Isometry3d& pose, double dt) { using namespace descartes_core; using namespace descartes_trajectory; @@ -185,11 +185,11 @@ std::vector makePath() const static int num_steps = 20; const static double time_between_points = 0.5; - EigenSTL::vector_Affine3d pattern_poses; + EigenSTL::vector_Isometry3d pattern_poses; for (int i = -num_steps / 2; i < num_steps / 2; ++i) { // Create a pose and initialize it to identity - Eigen::Affine3d pose = Eigen::Affine3d::Identity(); + Eigen::Isometry3d pose = Eigen::Isometry3d::Identity(); // set the translation (we're moving along a line in Y) pose.translation() = Eigen::Vector3d(0, i * step_size, 0); // set the orientation. By default, the tool will be pointing up into the air when we usually want it to @@ -200,7 +200,7 @@ std::vector makePath() // Now lets translate these points to Descartes trajectory points // The ABB2400 is pretty big, so let's move the path forward and up. - Eigen::Affine3d pattern_origin = Eigen::Affine3d::Identity(); + Eigen::Isometry3d pattern_origin = Eigen::Isometry3d::Identity(); pattern_origin.translation() = Eigen::Vector3d(1.0, 0, 0.3); std::vector result; diff --git a/descartes_tutorials/src/tutorial2.cpp b/descartes_tutorials/src/tutorial2.cpp index ca5b97e..215708a 100644 --- a/descartes_tutorials/src/tutorial2.cpp +++ b/descartes_tutorials/src/tutorial2.cpp @@ -140,9 +140,9 @@ int main(int argc, char** argv) * @brief Loads a specific file containing the perimeter of the puzzle part generated from CAD. The frames * are exported in the "puzzle" frame, so we can coordinate the motion of the robot with this part. */ -static EigenSTL::vector_Affine3d makePuzzleToolPoses() +static EigenSTL::vector_Isometry3d makePuzzleToolPoses() { - EigenSTL::vector_Affine3d path; // results + EigenSTL::vector_Isometry3d path; // results std::ifstream indata; // input file // You could load your parts from anywhere, but we are transporting them with the git repo @@ -182,7 +182,7 @@ static EigenSTL::vector_Affine3d makePuzzleToolPoses() Eigen::Vector3d temp_x = (-1 * pos).normalized(); Eigen::Vector3d y_axis = (norm.cross(temp_x)).normalized(); Eigen::Vector3d x_axis = (y_axis.cross(norm)).normalized(); - Eigen::Affine3d pose; + Eigen::Isometry3d pose; pose.matrix().col(0).head<3>() = x_axis; pose.matrix().col(1).head<3>() = y_axis; pose.matrix().col(2).head<3>() = norm; @@ -196,7 +196,7 @@ static EigenSTL::vector_Affine3d makePuzzleToolPoses() } std::vector -makeDescartesTrajectory(const EigenSTL::vector_Affine3d& path) +makeDescartesTrajectory(const EigenSTL::vector_Isometry3d& path) { using namespace descartes_core; using namespace descartes_trajectory; @@ -214,7 +214,7 @@ makeDescartesTrajectory(const EigenSTL::vector_Affine3d& path) listener.lookupTransform("world", "grinder_frame", ros::Time(0), grinder_frame); // Descartes uses eigen, so let's convert the data type - Eigen::Affine3d gf; + Eigen::Isometry3d gf; tf::transformTFToEigen(grinder_frame, gf); // When you tell Descartes about a "cartesian" frame, you can specify 4 "supporting" frames, and all are @@ -256,7 +256,7 @@ makeDescartesTrajectory(const EigenSTL::vector_Affine3d& path) std::vector makePath() { - EigenSTL::vector_Affine3d tool_poses = makePuzzleToolPoses(); + EigenSTL::vector_Isometry3d tool_poses = makePuzzleToolPoses(); return makeDescartesTrajectory(tool_poses); }