From e449158dc39383277868ab64a7270a8437611fc4 Mon Sep 17 00:00:00 2001 From: John Wason Date: Fri, 29 Nov 2024 22:24:57 -0500 Subject: [PATCH] Update examples --- .../tesseract_planning_example_composer.py | 26 +++++++++++----- .../tesseract_planning_example_no_composer.py | 14 +++++---- .../examples/abb_irb2400_viewer.py | 17 +++++++---- .../tesseract_material_mesh_viewer.py | 30 ++----------------- 4 files changed, 40 insertions(+), 47 deletions(-) diff --git a/examples/tesseract_planning_example_composer.py b/examples/tesseract_planning_example_composer.py index 9eb327392..be9775f58 100644 --- a/examples/tesseract_planning_example_composer.py +++ b/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)) diff --git a/examples/tesseract_planning_example_no_composer.py b/examples/tesseract_planning_example_no_composer.py index 76c3c9cb4..ad61003e1 100644 --- a/examples/tesseract_planning_example_no_composer.py +++ b/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() diff --git a/tesseract_viewer_python/examples/abb_irb2400_viewer.py b/tesseract_viewer_python/examples/abb_irb2400_viewer.py index eb31a60d6..e6e583fc5 100644 --- a/tesseract_viewer_python/examples/abb_irb2400_viewer.py +++ b/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) diff --git a/tesseract_viewer_python/examples/tesseract_material_mesh_viewer.py b/tesseract_viewer_python/examples/tesseract_material_mesh_viewer.py index fe48bccac..04e9d6058 100644 --- a/tesseract_viewer_python/examples/tesseract_material_mesh_viewer.py +++ b/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 @@ """ -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()