Merge pull request #15 from kkufieta/issue-14

Switching from Affine3d to Isometry3d following recent MoveIt changes in Melodic

descartes_tutorials/CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
   trajectory_msgs
   tf
   tf_conversions
+  eigen_conversions
 )
 
 catkin_package()

descartes_tutorials/src/tutorial1.cpp

@@ -152,15 +152,15 @@ 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;
 
   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<descartes_core::TrajectoryPtPtr> 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<descartes_core::TrajectoryPtPtr> 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<descartes_core::TrajectoryPtPtr> result;

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<descartes_core::TrajectoryPtPtr>
-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<descartes_core::TrajectoryPtPtr> makePath()
 {
-  EigenSTL::vector_Affine3d tool_poses = makePuzzleToolPoses();
+  EigenSTL::vector_Isometry3d tool_poses = makePuzzleToolPoses();
   return makeDescartesTrajectory(tool_poses);
 }