brush up before submitting draft PR

examples/glass_upright_example.py

@@ -44,11 +44,12 @@
     TaskComposerInput,
 )
 
-from examples.puzzle import TRAJOPT_DEFAULT_NAMESPACE
+from examples.puzzle_piece_example import TRAJOPT_DEFAULT_NAMESPACE
 from examples.utils import (
     get_environment,
     tesseract_task_composer_config_file,
     as_joint_trajectory,
+    print_joints,
 )
 from tesseract_viewer_python.tesseract_robotics_viewer import TesseractViewer
 
@@ -239,15 +240,8 @@ def plan(self):
 
         print(f"planning took {stop} seconds")
 
-        try:
-            # output_key = self.task.getOutputKeys()[0]
-            _ci: AnyPoly = self.task_data.getData(self.output_key)
-            ci = AnyPoly_as_CompositeInstruction(_ci)
-        except RuntimeError:
-            print("could not create a composite instruction from results")
-            raise
-        else:
-            trajectory: JointTrajectory = toJointTrajectory(ci)
+        if self.task_input.isSuccessful():
+            self.plot()
 
     # TODO: move to planner
     def _create_viewer(self):
@@ -296,7 +290,6 @@ def run():
 
     gue = GlassUprightExample(env, manip_info, True)
     gue.plan()
-    gue.plot()
 
     input("press enter to exit")
 

examples/puzzle.py → examples/puzzle_piece_example.py

@@ -12,9 +12,9 @@
 # https://vimeo.com/293314190 @ 4:27
 
 # TODO
-sys.path.extend(
-    ["Y:\CADCAM\tesseract_python\tesseract_viewer_python\tesseract_robotics_viewer"]
-)
+# sys.path.extend(
+#     ["Y:\CADCAM\tesseract_python\tesseract_viewer_python\tesseract_robotics_viewer"]
+# )
 
 from tesseract_viewer_python.tesseract_robotics_viewer import TesseractViewer
 
@@ -63,7 +63,11 @@
     PlanningTaskComposerProblemUPtr_as_TaskComposerProblemUPtr,
 )
 
-from utils import get_environment, tesseract_task_composer_config_file, as_joint_trajectory
+from utils import (
+    get_environment,
+    tesseract_task_composer_config_file,
+    as_joint_trajectory,
+)
 
 TRAJOPT_DEFAULT_NAMESPACE = "TrajOptMotionPlannerTask"
 
@@ -192,29 +196,23 @@ def add_poses(self, tool_poses):
             self.program.appendMoveInstruction(plan_instruction)
 
     def plan(self) -> AnyPoly_as_CompositeInstruction:
-
         # Assign the current state as the seed for cartesian waypoints
         assignCurrentStateAsSeed(self.program, self.t_env)
 
         start = time.time()
         # Run the task and wait for completion
-        future = self.task_executor.plan(self.task.get(), self.task_input)
+        future = self.task_executor.run(self.task.get(), self.task_input)
         future.wait()
         stop = time.time() - start
 
+        is_aborted = self.task_input.isAborted()
+        is_successful = self.task_input.isSuccessful()
+
         print(f"Planning took {stop} seconds.")
+        print(f"was planning aborted? {is_aborted}")
+        print(f"was planning succesful? {is_successful}")
 
-        # Retrieve the output, converting the AnyPoly back to a CompositeInstruction
-        try:
-            results = AnyPoly_as_CompositeInstruction(
-                self.task_input.data_storage.getData(self.output_key)
-            )
-        except RuntimeError as e:
-            print(e)
-            return None
-        else:
-            print(results)
-            return results
+        return is_aborted, is_successful
 
 
 def make_puzzle_tool_poses() -> list[Isometry3d]:
@@ -245,7 +243,7 @@ def make_puzzle_tool_poses() -> list[Isometry3d]:
 
         norm.unitize()
 
-        temp_x = (pos*-1).unitized()
+        temp_x = (pos * -1).unitized()
         y_axis = norm.cross(temp_x).unitized()
         x_axis = y_axis.cross(norm).unitized()
 
@@ -307,18 +305,12 @@ def run(self):
 
         profile_dict = create_trajopt_profile()
 
-        pl = Planner(mi, env, profile_dict)
-        pl.add_poses(tool_poses)
-        pl.create_task()
-        results = pl.plan()
+        self.planner = Planner(mi, env, profile_dict)
+        self.planner.add_poses(tool_poses)
+        self.planner.create_task()
+        is_aborted, is_successful = self.planner.plan()
 
-        joint_trajectory = as_joint_trajectory(pl.task, pl.task_data)
-
-        # print("Final trajectory is collision-free")
-        # return input.isSuccessful()
-
-        # return results
-        return joint_trajectory
+        return is_aborted, is_successful
 
 
 if __name__ == "__main__":
@@ -338,11 +330,14 @@ def run(self):
     viewer.start_serve_background()
 
     ppe = PuzzlePieceExample(env)
-    results = ppe.run()
+    is_aborted, is_successful = ppe.run()
+
+    if not is_aborted and is_successful:
+        joint_trajectory = as_joint_trajectory(ppe.planner.task, ppe.planner.task_data)
 
-    # Update the viewer with the results to animate the trajectory
-    # Open web browser to http://localhost:8000 to view the results
-    viewer.update_trajectory(results)
-    viewer.plot_trajectory(results, manip_info)
+        # Update the viewer with the results to animate the trajectory
+        # Open web browser to http://localhost:8000 to view the results
+        viewer.update_trajectory(joint_trajectory)
+        viewer.plot_trajectory(joint_trajectory, manip_info)
 
-    input("press to exit the viewer ( http://localhost:8000 )")
+        input("press to exit the viewer ( http://localhost:8000 )")

examples/utils.py

@@ -3,15 +3,23 @@
 import numpy as np
 from tesseract_robotics.tesseract_command_language import (
     InstructionPoly_as_MoveInstructionPoly,
-    WaypointPoly_as_StateWaypointPoly, AnyPoly_as_CompositeInstruction, toJointTrajectory,
+    WaypointPoly_as_StateWaypointPoly,
+    AnyPoly_as_CompositeInstruction,
+    toJointTrajectory,
 )
 from tesseract_robotics.tesseract_common import (
     FilesystemPath,
     ManipulatorInfo,
-    GeneralResourceLocator, Isometry3d, JointTrajectory, AnyPoly,
+    GeneralResourceLocator,
+    Isometry3d,
+    JointTrajectory,
+    AnyPoly,
 )
 from tesseract_robotics.tesseract_environment import Environment
-from tesseract_robotics.tesseract_task_composer import TaskComposerDataStorage, TaskComposerNode
+from tesseract_robotics.tesseract_task_composer import (
+    TaskComposerDataStorage,
+    TaskComposerNode,
+)
 
 TESSERACT_SUPPORT_DIR = os.environ["TESSERACT_RESOURCE_PATH"]
 TESSERACT_TASK_COMPOSER_DIR = os.environ["TESSERACT_TASK_COMPOSER_CONFIG_FILE"]
@@ -20,10 +28,9 @@
 
 
 def tesseract_task_composer_config_file():
-    # OVERRIDE defaults that has no IPOPT trajopt
-    # TODO
     config_path = FilesystemPath(
-        "Y:\\CADCAM\\tesseract_planning\\tesseract_task_composer\\config\\task_composer_plugins.yaml"
+        # "Y:\\CADCAM\\tesseract_planning\\tesseract_task_composer\\config\\task_composer_plugins.yaml"
+        TESSERACT_TASK_COMPOSER_DIR
     )
     return config_path
 
@@ -74,11 +81,13 @@ def print_joints(results):
 
 def _translation(p) -> Isometry3d:
     H = np.eye(4)
-    H[0:3,3] = p
+    H[0:3, 3] = p
     return Isometry3d(H)
 
 
-def as_joint_trajectory(task: TaskComposerNode, tcds: TaskComposerDataStorage) -> JointTrajectory:
+def as_joint_trajectory(
+    task: TaskComposerNode, tcds: TaskComposerDataStorage
+) -> JointTrajectory:
     # Plot Process Trajectory
     # TODO as composite instruction
 
@@ -89,4 +98,3 @@ def as_joint_trajectory(task: TaskComposerNode, tcds: TaskComposerDataStorage) -
 
     trajectory: JointTrajectory = toJointTrajectory(ci)
     return trajectory
-