Skip to content

Commit

Permalink
Update examples
Browse files Browse the repository at this point in the history
  • Loading branch information
johnwason committed Nov 30, 2024
1 parent a6fd2d5 commit e449158
Show file tree
Hide file tree
Showing 4 changed files with 40 additions and 47 deletions.
26 changes: 18 additions & 8 deletions examples/tesseract_planning_example_composer.py
Original file line number Diff line number Diff line change
Expand Up @@ -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, \
Expand All @@ -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
Expand Down Expand Up @@ -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.
Expand All @@ -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))

Expand Down
14 changes: 9 additions & 5 deletions examples/tesseract_planning_example_no_composer.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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()
Expand Down
17 changes: 11 additions & 6 deletions tesseract_viewer_python/examples/abb_irb2400_viewer.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand Down
30 changes: 2 additions & 28 deletions tesseract_viewer_python/examples/tesseract_material_mesh_viewer.py
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -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()
Expand Down

0 comments on commit e449158

Please sign in to comment.