Update examples

Author: johnwason

examples/tesseract_planning_example_composer.py

@@ -5,7 +5,7 @@
 import numpy.testing as nptest
 
 from tesseract_robotics.tesseract_common import GeneralResourceLocator
-from tesseract_robotics.tesseract_environment import Environment
+from tesseract_robotics.tesseract_environment import Environment, AnyPoly_wrap_EnvironmentConst
 from tesseract_robotics.tesseract_common import FilesystemPath, Isometry3d, Translation3d, Quaterniond, \
     ManipulatorInfo, AnyPoly, AnyPoly_wrap_double
 from tesseract_robotics.tesseract_command_language import CartesianWaypoint, WaypointPoly, \
@@ -15,9 +15,10 @@
         AnyPoly_wrap_CompositeInstruction, DEFAULT_PROFILE_KEY, JointWaypoint, JointWaypointPoly, \
         InstructionPoly_as_MoveInstructionPoly, WaypointPoly_as_StateWaypointPoly, \
         MoveInstructionPoly_wrap_MoveInstruction, StateWaypointPoly_wrap_StateWaypoint, \
-        CartesianWaypointPoly_wrap_CartesianWaypoint, JointWaypointPoly_wrap_JointWaypoint
+        CartesianWaypointPoly_wrap_CartesianWaypoint, JointWaypointPoly_wrap_JointWaypoint, \
+        AnyPoly_wrap_ProfileDictionary
 
-from tesseract_robotics.tesseract_task_composer import TaskComposerPluginFactory, PlanningTaskComposerProblem, \
+from tesseract_robotics.tesseract_task_composer import TaskComposerPluginFactory, \
     TaskComposerDataStorage, TaskComposerContext
 
 from tesseract_robotics_viewer import TesseractViewer
@@ -134,7 +135,8 @@
 task = factory.createTaskComposerNode("FreespacePipeline")
 
 # Get the output keys for the task
-output_key = task.getOutputKeys()[0]
+output_key = task.getOutputKeys().get("program")
+input_key = task.getInputKeys().get("planning_input")
 
 # Create a profile dictionary. Profiles can be customized by adding to this dictionary and setting the profiles
 # in the instructions.
@@ -143,18 +145,26 @@
 # Create an AnyPoly containing the program. This explicit step is required because the Python bindings do not
 # support implicit conversion from the CompositeInstruction to the AnyPoly.
 program_anypoly = AnyPoly_wrap_CompositeInstruction(program)
+environment_anypoly = AnyPoly_wrap_EnvironmentConst(t_env)
+profiles_anypoly = AnyPoly_wrap_ProfileDictionary(profiles)
 
-# Create the task problem and input
-task_planning_problem = PlanningTaskComposerProblem(t_env, profiles)
-task_planning_problem.input = program_anypoly
+# Create the task data
+task_data = TaskComposerDataStorage()
+task_data.setData(input_key, program_anypoly)
+task_data.setData("environment", environment_anypoly)
+task_data.setData("profiles", profiles_anypoly)
 
 # Create an executor to run the task
 task_executor = factory.createTaskComposerExecutor("TaskflowExecutor")
 
 # Run the task and wait for completion
-future = task_executor.run(task.get(), task_planning_problem)
+future = task_executor.run(task.get(), task_data)
 future.wait()
 
+if not future.context.isSuccessful():
+    print("Planning task failed")
+    exit(1)
+
 # Retrieve the output, converting the AnyPoly back to a CompositeInstruction
 results = AnyPoly_as_CompositeInstruction(future.context.data_storage.getData(output_key))
 

examples/tesseract_planning_example_no_composer.py

@@ -11,11 +11,11 @@
 from tesseract_robotics.tesseract_motion_planners import PlannerRequest, PlannerResponse
 from tesseract_robotics.tesseract_motion_planners_simple import generateInterpolatedProgram
 from tesseract_robotics.tesseract_motion_planners_ompl import OMPLDefaultPlanProfile, RRTConnectConfigurator, \
-    OMPLProblemGeneratorFn, OMPLMotionPlanner, ProfileDictionary_addProfile_OMPLPlanProfile
+    OMPLMotionPlanner, ProfileDictionary_addProfile_OMPLPlanProfile
 from tesseract_robotics.tesseract_time_parameterization import TimeOptimalTrajectoryGeneration, \
     InstructionsTrajectory
 from tesseract_robotics.tesseract_motion_planners_trajopt import TrajOptDefaultPlanProfile, TrajOptDefaultCompositeProfile, \
-    TrajOptProblemGeneratorFn, TrajOptMotionPlanner, ProfileDictionary_addProfile_TrajOptPlanProfile, \
+    TrajOptMotionPlanner, ProfileDictionary_addProfile_TrajOptPlanProfile, \
     ProfileDictionary_addProfile_TrajOptCompositeProfile
 
 import os
@@ -167,9 +167,13 @@
 # output program since the input is modified.
 time_parameterization = TimeOptimalTrajectoryGeneration()
 instructions_trajectory = InstructionsTrajectory(trajopt_results_instruction)
-max_velocity = np.array([2.088, 2.082, 3.27, 3.6, 3.3, 3.078],dtype=np.float64)
-max_acceleration = np.array([ 1, 1, 1, 1, 1, 1],dtype=np.float64)
-assert time_parameterization.computeTimeStamps(instructions_trajectory, max_velocity, max_acceleration)
+max_velocity = np.array([[2.088, 2.082, 3.27, 3.6, 3.3, 3.078]],dtype=np.float64)
+max_velocity = np.hstack((-max_velocity.T, max_velocity.T))
+max_acceleration = np.array([[ 1, 1, 1, 1, 1, 1]],dtype=np.float64)
+max_acceleration = np.hstack((-max_acceleration.T, max_acceleration.T))
+max_jerk = np.array([[ 1, 1, 1, 1, 1, 1]],dtype=np.float64)
+max_jerk = np.hstack((-max_jerk.T, max_jerk.T))
+assert time_parameterization.compute(instructions_trajectory, max_velocity, max_acceleration, max_jerk)
 
 # Flatten the results into a single list of instructions
 trajopt_results = trajopt_results_instruction.flatten()

tesseract_viewer_python/examples/abb_irb2400_viewer.py

@@ -7,13 +7,14 @@
     CompositeInstruction, MoveInstructionPoly, CartesianWaypointPoly, ProfileDictionary, \
     CartesianWaypointPoly_wrap_CartesianWaypoint, MoveInstructionPoly_wrap_MoveInstruction
 
-from tesseract_robotics.tesseract_motion_planners import PlannerRequest, PlannerResponse, generateInterpolatedProgram
+from tesseract_robotics.tesseract_motion_planners import PlannerRequest, PlannerResponse
+from tesseract_robotics.tesseract_motion_planners_simple import generateInterpolatedProgram
 from tesseract_robotics.tesseract_motion_planners_ompl import OMPLDefaultPlanProfile, RRTConnectConfigurator, \
-    OMPLProblemGeneratorFn, OMPLMotionPlanner, ProfileDictionary_addProfile_OMPLPlanProfile
+    OMPLMotionPlanner, ProfileDictionary_addProfile_OMPLPlanProfile
 from tesseract_robotics.tesseract_time_parameterization import TimeOptimalTrajectoryGeneration, \
     InstructionsTrajectory
 from tesseract_robotics.tesseract_motion_planners_trajopt import TrajOptDefaultPlanProfile, TrajOptDefaultCompositeProfile, \
-    TrajOptProblemGeneratorFn, TrajOptMotionPlanner, ProfileDictionary_addProfile_TrajOptPlanProfile, \
+    TrajOptMotionPlanner, ProfileDictionary_addProfile_TrajOptPlanProfile, \
     ProfileDictionary_addProfile_TrajOptCompositeProfile
 
 import os
@@ -115,9 +116,13 @@
 
 time_parameterization = TimeOptimalTrajectoryGeneration()
 instructions_trajectory = InstructionsTrajectory(trajopt_results_instruction)
-max_velocity = np.array([2.088, 2.082, 3.27, 3.6, 3.3, 3.078],dtype=np.float64)
-max_acceleration = np.array([ 1, 1, 1, 1, 1, 1],dtype=np.float64)
-assert time_parameterization.computeTimeStamps(instructions_trajectory, max_velocity, max_acceleration)
+max_velocity = np.array([[2.088, 2.082, 3.27, 3.6, 3.3, 3.078]],dtype=np.float64)
+max_velocity = np.hstack((-max_velocity.T, max_velocity.T))
+max_acceleration = np.array([[ 1, 1, 1, 1, 1, 1]],dtype=np.float64)
+max_acceleration = np.hstack((-max_acceleration.T, max_acceleration.T))
+max_jerk = np.array([[ 1, 1, 1, 1, 1, 1]],dtype=np.float64)
+max_jerk = np.hstack((-max_jerk.T, max_jerk.T))
+assert time_parameterization.compute(instructions_trajectory, max_velocity, max_acceleration, max_jerk)
 
 trajopt_results = trajopt_results_instruction.flatten()
 viewer.update_trajectory(trajopt_results)

tesseract_viewer_python/examples/tesseract_material_mesh_viewer.py

@@ -1,5 +1,5 @@
 from tesseract_robotics.tesseract_environment import Environment
-from tesseract_robotics.tesseract_common import ResourceLocator, SimpleLocatedResource
+from tesseract_robotics.tesseract_common import GeneralResourceLocator
 import os
 import re
 import traceback
@@ -47,36 +47,10 @@
 </robot>
 """
 
-TESSERACT_SUPPORT_DIR = os.environ["TESSERACT_SUPPORT_DIR"]
-
-class TesseractSupportResourceLocator(ResourceLocator):
-    def __init__(self):
-        super().__init__()
-    
-    def locateResource(self, url):
-        try:
-            try:
-                if os.path.exists(url):
-                    return SimpleLocatedResource(url, url, self)
-            except:
-                pass
-            url_match = re.match(r"^package:\/\/tesseract_support\/(.*)$",url)
-            if (url_match is None):
-                print("url_match failed")
-                return None
-            if not "TESSERACT_SUPPORT_DIR" in os.environ:
-                return None
-            tesseract_support = os.environ["TESSERACT_SUPPORT_DIR"]
-            filename = os.path.join(tesseract_support, os.path.normpath(url_match.group(1)))
-            ret = SimpleLocatedResource(url, filename, self)
-            return ret
-        except:
-            traceback.print_exc()
-
 t_env = Environment()
 
 # locator_fn must be kept alive by maintaining a reference
-locator=TesseractSupportResourceLocator()
+locator=GeneralResourceLocator()
 t_env.init(shapes_urdf, locator)
 
 viewer = TesseractViewer()