Updates to generators and examples to utilize cartesian pose getParen…

…tTransform method
tesseract-robotics · Sep 17, 2019 · a3c9b7b · a3c9b7b
1 parent 0fa39a2
commit a3c9b7b
Show file tree

Hide file tree

Showing 6 changed files with 24 additions and 14 deletions.
diff --git a/...sseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/core/utils.h b/...sseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/core/utils.h
@@ -102,7 +102,7 @@ inline std::vector<Waypoint::Ptr> interpolate(const Waypoint& start, const Waypo
       for (auto& eigen_pose : eigen_poses)
       {
         CartesianWaypoint::Ptr new_waypoint =
-            std::make_shared<tesseract_motion_planners::CartesianWaypoint>(eigen_pose);
+            std::make_shared<tesseract_motion_planners::CartesianWaypoint>(eigen_pose, start.getParentLinkName());
         new_waypoint->setCoefficients(start.getCoefficients());
         new_waypoint->setIsCritical(start.isCritical());
         result.push_back(new_waypoint);

diff --git a/...process_planners/include/tesseract_process_planners/generators/axial_approach_generator.h b/...process_planners/include/tesseract_process_planners/generators/axial_approach_generator.h
@@ -33,7 +33,7 @@ class AxialApproachGenerator : public ProcessStepGenerator
 
       tesseract_motion_planners::CartesianWaypoint::Ptr new_waypoint =
           std::make_shared<tesseract_motion_planners::CartesianWaypoint>(
-              config.world_offset_direction * cur_waypoint->getTransform() * config.local_offset_direction * scaled);
+              config.world_offset_direction * cur_waypoint->getTransform() * config.local_offset_direction * scaled, cur_waypoint->getParentLinkName());
       new_waypoint->setCoefficients(cur_waypoint->getCoefficients());
       new_waypoint->setIsCritical(cur_waypoint->isCritical());
       approach.push_back(new_waypoint);

diff --git a/...rocess_planners/include/tesseract_process_planners/generators/axial_departure_generator.h b/...rocess_planners/include/tesseract_process_planners/generators/axial_departure_generator.h
@@ -33,7 +33,7 @@ class AxialDepartureGenerator : public ProcessStepGenerator
 
       tesseract_motion_planners::CartesianWaypoint::Ptr new_waypoint =
           std::make_shared<tesseract_motion_planners::CartesianWaypoint>(
-              config.world_offset_direction * cur_waypoint->getTransform() * config.local_offset_direction * scaled);
+              config.world_offset_direction * cur_waypoint->getTransform() * config.local_offset_direction * scaled, cur_waypoint->getParentLinkName());
       new_waypoint->setCoefficients(cur_waypoint->getCoefficients());
       new_waypoint->setIsCritical(cur_waypoint->isCritical());
       departure.push_back(new_waypoint);

diff --git a/...ss_planners/include/tesseract_process_planners/generators/passthrough_process_generator.h b/...ss_planners/include/tesseract_process_planners/generators/passthrough_process_generator.h
@@ -24,7 +24,7 @@ class PassthroughProcessGenerator : public ProcessStepGenerator
           std::static_pointer_cast<tesseract_motion_planners::CartesianWaypoint>(waypoint);
       tesseract_motion_planners::CartesianWaypoint::Ptr new_waypoint =
           std::make_shared<tesseract_motion_planners::CartesianWaypoint>(
-              config.world_offset_direction * cur_waypoint->getTransform() * config.local_offset_direction);
+              config.world_offset_direction * cur_waypoint->getTransform() * config.local_offset_direction, cur_waypoint->getParentLinkName());
       new_waypoint->setCoefficients(cur_waypoint->getCoefficients());
       new_waypoint->setIsCritical(cur_waypoint->isCritical());
       new_waypoints.push_back(new_waypoint);

diff --git a/tesseract_ros/tesseract_examples/tesseract_ros_examples/config/basic_cartesian_plan.json b/tesseract_ros/tesseract_examples/tesseract_ros_examples/config/basic_cartesian_plan.json
@@ -37,7 +37,8 @@
         "wxyz" : [0.0, 0.0, 1.0, 0.0],
         "link" : "tool0",
         "pos_coeffs" : [10, 10, 10],
-        "rot_coeffs" : [10, 10, 10]
+        "rot_coeffs" : [10, 10, 10],
+        "target" : "world"
       }
     },
     {
@@ -50,7 +51,8 @@
         "wxyz" : [0.0, 0.0, 1.0, 0.0],
         "link" : "tool0",
         "pos_coeffs" : [10, 10, 10],
-        "rot_coeffs" : [10, 10, 10]
+        "rot_coeffs" : [10, 10, 10],
+        "target" : "world"
       }
     },
     {
@@ -63,7 +65,8 @@
         "wxyz" : [0.0, 0.0, 1.0, 0.0],
         "link" : "tool0",
         "pos_coeffs" : [10, 10, 10],
-        "rot_coeffs" : [10, 10, 10]
+        "rot_coeffs" : [10, 10, 10],
+        "target" : "world"
       }
     },
     {
@@ -76,7 +79,8 @@
         "wxyz" : [0.0, 0.0, 1.0, 0.0],
         "link" : "tool0",
         "pos_coeffs" : [10, 10, 10],
-        "rot_coeffs" : [10, 10, 10]
+        "rot_coeffs" : [10, 10, 10],
+        "target" : "world"
       }
     },
     {
@@ -89,7 +93,8 @@
         "wxyz" : [0.0, 0.0, 1.0, 0.0],
         "link" : "tool0",
         "pos_coeffs" : [10, 10, 10],
-        "rot_coeffs" : [10, 10, 10]
+        "rot_coeffs" : [10, 10, 10],
+        "target" : "world"
       }
     }
   ],

diff --git a/tesseract_ros/tesseract_examples/tesseract_ros_examples/config/glass_up_right_plan.json b/tesseract_ros/tesseract_examples/tesseract_ros_examples/config/glass_up_right_plan.json
@@ -52,7 +52,8 @@
         "wxyz" : [0.0, 0.0, 1.0, 0.0],
         "link" : "tool0",
         "pos_coeffs" : [10, 10, 10],
-        "rot_coeffs" : [10, 10, 10]
+        "rot_coeffs" : [10, 10, 10],
+        "target" : "world"
       }
     },
     {
@@ -65,7 +66,8 @@
         "wxyz" : [0.0, 0.0, 1.0, 0.0],
         "link" : "tool0",
         "pos_coeffs" : [0, 0, 0],
-        "rot_coeffs" : [10, 10, 0]
+        "rot_coeffs" : [10, 10, 0],
+        "target" : "world"
       }
     },
     {
@@ -78,7 +80,8 @@
         "wxyz" : [0.0, 0.0, 1.0, 0.0],
         "link" : "tool0",
         "pos_coeffs" : [0, 0, 0],
-        "rot_coeffs" : [10, 10, 0]
+        "rot_coeffs" : [10, 10, 0],
+        "target" : "world"
       }
     },
     {
@@ -91,7 +94,8 @@
         "wxyz" : [0.0, 0.0, 1.0, 0.0],
         "link" : "tool0",
         "pos_coeffs" : [0, 0, 0],
-        "rot_coeffs" : [10, 10, 0]
+        "rot_coeffs" : [10, 10, 0],
+        "target" : "world"
       }
     },
     {
@@ -104,7 +108,8 @@
         "wxyz" : [0.0, 0.0, 1.0, 0.0],
         "link" : "tool0",
         "pos_coeffs" : [10, 10, 10],
-        "rot_coeffs" : [10, 10, 0]
+        "rot_coeffs" : [10, 10, 0],
+        "target" : "world"
       }
     }
   ],