From ae83e49480cfa19519864ad06db0395872c31ca2 Mon Sep 17 00:00:00 2001 From: Jelle Feringa Date: Tue, 19 Sep 2023 14:45:10 +0200 Subject: [PATCH 1/4] porting the puzzle example from C++ --- examples/puzzle.py | 358 +++++++++++++++++++++++++++++++++++++++++++++ examples/utils.py | 58 ++++++++ 2 files changed, 416 insertions(+) create mode 100644 examples/puzzle.py create mode 100644 examples/utils.py diff --git a/examples/puzzle.py b/examples/puzzle.py new file mode 100644 index 00000000..d17bda42 --- /dev/null +++ b/examples/puzzle.py @@ -0,0 +1,358 @@ +import time +import os, sys + +from compas.geometry import Vector + +# this example is transliterated from C++, you'll find the original here: +# https://github.com/tesseract-robotics/tesseract_planning/blob/master/tesseract_examples/src/puzzle_piece_example.cpp +# For some context of this particular example, see ROSCon 2018 Madrid: Optimization Motion +# Planning with Tesseract and TrajOpt for Industrial Applications +# https://vimeo.com/293314190 @ 4:27 + +# TODO +sys.path.extend( + ["Y:\CADCAM\tesseract_python\tesseract_viewer_python\tesseract_robotics_viewer"] +) + +from tesseract_viewer_python.tesseract_robotics_viewer import TesseractViewer + +# TODO +os.environ["TRAJOPT_LOG_THRESH"] = "DEBUG" + +import numpy as np +from tesseract_robotics.tesseract_command_language import ( + ProfileDictionary, + CompositeInstruction, + CartesianWaypoint, + MoveInstruction, + CartesianWaypointPoly_wrap_CartesianWaypoint, + MoveInstructionPoly_wrap_MoveInstruction, + MoveInstructionType_LINEAR, + AnyPoly_wrap_CompositeInstruction, + AnyPoly_as_CompositeInstruction, + toJointTrajectory, +) +from tesseract_robotics.tesseract_common import ( + Isometry3d, + ManipulatorInfo, + GeneralResourceLocator, + JointTrajectory, + AnyPoly, +) +from tesseract_robotics.tesseract_environment import Environment +from tesseract_robotics.tesseract_motion_planners import assignCurrentStateAsSeed +from tesseract_robotics.tesseract_motion_planners_trajopt import ( + TrajOptDefaultPlanProfile, + TrajOptDefaultCompositeProfile, + TrajOptDefaultSolverProfile, + ProfileDictionary_addProfile_TrajOptPlanProfile, + ProfileDictionary_addProfile_TrajOptCompositeProfile, + ProfileDictionary_addProfile_TrajOptSolverProfile, + BasicTrustRegionSQPParameters, + ModelType, + CollisionEvaluatorType_SINGLE_TIMESTEP, +) + +from tesseract_robotics.tesseract_task_composer import ( + TaskComposerPluginFactory, + TaskComposerDataStorage, + TaskComposerInput, + PlanningTaskComposerProblemUPtr, + PlanningTaskComposerProblemUPtr_as_TaskComposerProblemUPtr, +) + +from utils import get_environment, tesseract_task_composer_config_file + +TRAJOPT_DEFAULT_NAMESPACE = "TrajOptMotionPlannerTask" + + +def create_trajopt_profile() -> ProfileDictionary: + # Create TrajOpt Profile + trajopt_plan_profile = TrajOptDefaultPlanProfile() + trajopt_plan_profile.cartesian_coeff = np.array( + [10, 10, 10, 10, 10, 0], dtype=np.float64 + ) + + trajopt_composite_profile = TrajOptDefaultCompositeProfile() + trajopt_composite_profile.collision_constraint_config.enabled = False + trajopt_composite_profile.collision_cost_config.enabled = True + trajopt_composite_profile.collision_cost_config.safety_margin = 0.025 + trajopt_composite_profile.collision_cost_config.type = ( + CollisionEvaluatorType_SINGLE_TIMESTEP + ) + trajopt_composite_profile.collision_cost_config.coeff = 20 + + trajopt_solver_profile = TrajOptDefaultSolverProfile() + + btr_params = BasicTrustRegionSQPParameters() + btr_params.max_iter = 200 + btr_params.min_approx_improve = 1e-3 + btr_params.min_trust_box_size = 1e-3 + + mt = ModelType(ModelType.OSQP) + # seems to do its job: fails when I set gurobi; not build with that options + # mt = ModelType(ModelType.GUROBI) + + trajopt_solver_profile.opt_info = btr_params + trajopt_solver_profile.convex_solver = mt + + # Create profile dictionary + trajopt_profiles = ProfileDictionary() + ProfileDictionary_addProfile_TrajOptPlanProfile( + trajopt_profiles, TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile + ) + + ProfileDictionary_addProfile_TrajOptSolverProfile( + trajopt_profiles, TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile + ) + + ProfileDictionary_addProfile_TrajOptCompositeProfile( + trajopt_profiles, + TRAJOPT_DEFAULT_NAMESPACE, + "DEFAULT", + trajopt_composite_profile, + ) + return trajopt_profiles + + +def _move_instruction_from_iso( + goal: Isometry3d, move_type=MoveInstructionType_LINEAR +) -> MoveInstructionPoly_wrap_MoveInstruction: + cwp_cw = CartesianWaypointPoly_wrap_CartesianWaypoint(CartesianWaypoint(goal)) + mip_mi = MoveInstructionPoly_wrap_MoveInstruction( + MoveInstruction(cwp_cw, move_type, "CARTESIAN") + ) + return mip_mi + + +class Planner: + def __init__( + self, mi: ManipulatorInfo, t_env: Environment, profile_dict: ProfileDictionary + ): + # Create Task Composer Plugin Factory + fs_pth = tesseract_task_composer_config_file() + self.factory = TaskComposerPluginFactory(fs_pth) + + # Create Program + self.program = CompositeInstruction("DEFAULT") + self.program.setManipulatorInfo(mi) + self.t_env = t_env + + self.profiles = profile_dict + self.task_data = TaskComposerDataStorage() + + def create_task(self): + """create task after all poses have been added""" + # 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. + self.program_anypoly = AnyPoly_wrap_CompositeInstruction(self.program) + + # Create the task composer node. In this case the FreespacePipeline is used. Many other are available. + # self.task = self.factory.createTaskComposerNode("FreespacePipeline") + + # # Create task + self.task = self.factory.createTaskComposerNode("TrajOptPipeline") + self.input_key = self.task.getInputKeys()[0] + self.output_key = self.task.getOutputKeys()[0] + + # Create Task Input Data + self.input_data = TaskComposerDataStorage() + self.input_data.setData(self.input_key, self.program_anypoly) + + # Create the task data storage and set the data + self.task_data.setData(self.input_key, self.program_anypoly) + + # Create the task problem and input + # self.problem = PlanningTaskComposerProblem( + # self.t_env, + # self.input_data, + # self.profiles + # ) + + # Create the task problem and input + self.task_planning_problem = PlanningTaskComposerProblemUPtr.make_unique( + self.t_env, self.task_data, self.profiles + ) + + self.task_problem = PlanningTaskComposerProblemUPtr_as_TaskComposerProblemUPtr( + self.task_planning_problem + ) + self.task_input = TaskComposerInput(self.task_problem) + + # Create an executor to run the task + self.task_executor = self.factory.createTaskComposerExecutor("TaskflowExecutor") + + def add_poses(self, tool_poses): + # Create cartesian waypoint + for n, i in enumerate(tool_poses): + plan_instruction = _move_instruction_from_iso(i) + plan_instruction.setDescription(f"waypoint_{n}") + 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.run(self.task.get(), self.task_input) + future.wait() + stop = time.time() - start + + print(f"Planning took {stop} seconds.") + + # 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 + + +def as_joint_trajectory(planner: Planner) -> JointTrajectory: + # Plot Process Trajectory + # TODO as composite instruction + _ci: AnyPoly = planner.input_data.getData(planner.output_key) + + ci = AnyPoly_as_CompositeInstruction(_ci) + + trajectory: JointTrajectory = toJointTrajectory(ci) + state_solver = planner.t_env.getStateSolver() + return trajectory + + +def make_puzzle_tool_poses() -> list[Isometry3d]: + path = [] # results + locator = GeneralResourceLocator() + # Locate the CSV file using the provided resource locator + resource = locator.locateResource( + "package://tesseract_support/urdf/puzzle_bent.csv" + ) + file_path = resource.getFilePath() + + # Open the CSV file for reading + with open(file_path, "r") as indata: + lines = indata.readlines() + + for lnum, line in enumerate(lines): + if lnum < 2: + continue + + cells = line.split(",") + xyzijk = [float(cell) for cell in cells[1:]] # Ignore the first value + + pos = Vector(xyzijk[0], xyzijk[1], xyzijk[2]) # Convert from mm to meters + pos /= 1000 + + print(pos) + norm = Vector(xyzijk[3], xyzijk[4], xyzijk[5]) + + norm.unitize() + + temp_x = (Vector(-1, -1, -1) * pos).unitized() + y_axis = norm.cross(temp_x).unitized() + x_axis = y_axis.cross(norm).unitized() + + # Create an Isometry3d pose + pose = Isometry3d() + mat = pose.matrix() + + mat[0][:3] = x_axis + mat[1][:3] = y_axis + mat[2][:3] = norm + mat[3][:3] = pos + + path.append(pose) + + return path + + +# path = ResourceLocator() + + +def create_manip() -> ManipulatorInfo: + # Create manipulator information for the program + mi = ManipulatorInfo() + mi.manipulator = "manipulator" + mi.working_frame = "part" + mi.tcp_frame = "grinder_frame" + return mi + + +class PuzzlePieceExample: + def __init__(self, env, plotter=None): + self.env = env + self.plotter = plotter + + def run(self): + if self.plotter is not None: + self.plotter.waitForConnection() + + # Set the robot initial state + joint_names = [ + "joint_a1", + "joint_a2", + "joint_a3", + "joint_a4", + "joint_a5", + "joint_a6", + "joint_a7", + ] + joint_pos = [-0.785398, 0.4, 0.0, -1.9, 0.0, 1.0, 0.0] + + # joint_state = JointState(joint_names, joint_pos) + + self.env.setState(joint_names, np.array(joint_pos)) + + mi = create_manip() + + # Get Tool Poses + tool_poses = make_puzzle_tool_poses() + + profile_dict = create_trajopt_profile() + + pl = Planner(mi, env, profile_dict) + pl.add_poses(tool_poses) + pl.create_task() + results = pl.plan() + + joint_trajectory = as_joint_trajectory(pl) + + # print("Final trajectory is collision-free") + # return input.isSuccessful() + + # return results + return joint_trajectory + + +if __name__ == "__main__": + make_puzzle_tool_poses() + env, manip_info, joint_names = get_environment( + "package://tesseract_support/urdf/puzzle_piece_workcell" + ) + + # Create a viewer and set the environment so the results can be displayed later + viewer = TesseractViewer() + viewer.update_environment(env, [0, 0, 0]) + + # Set the initial state of the robot + viewer.update_joint_positions(joint_names, np.ones(len(joint_names)) * 0.1) + + # Start the viewer + viewer.start_serve_background() + + ppe = PuzzlePieceExample(env) + results = ppe.run() + + # 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) + + input("press to exit the viewer ( http://localhost:8000 )") diff --git a/examples/utils.py b/examples/utils.py new file mode 100644 index 00000000..9c6887c5 --- /dev/null +++ b/examples/utils.py @@ -0,0 +1,58 @@ +import os + +from tesseract_robotics.tesseract_command_language import InstructionPoly_as_MoveInstructionPoly, \ + WaypointPoly_as_StateWaypointPoly +from tesseract_robotics.tesseract_common import FilesystemPath, ManipulatorInfo, GeneralResourceLocator +from tesseract_robotics.tesseract_environment import Environment + +TESSERACT_SUPPORT_DIR = os.environ['TESSERACT_RESOURCE_PATH'] +TESSERACT_TASK_COMPOSER_DIR = os.environ['TESSERACT_TASK_COMPOSER_CONFIG_FILE'] + +task_composer_filename = os.environ["TESSERACT_TASK_COMPOSER_CONFIG_FILE"] + +def tesseract_task_composer_config_file(): + # OVERRIDE defaults that has no IPOPT trajopt + config_path = FilesystemPath( + "Y:\\CADCAM\\tesseract_planning\\tesseract_task_composer\\config\\task_composer_plugins.yaml" + ) + return config_path + +def support_dir(pth): + return FilesystemPath(os.path.join(TESSERACT_SUPPORT_DIR, pth)) + + +def compose_dir(pth): + return FilesystemPath(os.path.join(TESSERACT_TASK_COMPOSER_DIR, pth)) + + +def get_environment(url) -> tuple[Environment, ManipulatorInfo, list]: + locator = GeneralResourceLocator() + env = Environment() + #tesseract_support = os.environ["TESSERACT_SUPPORT_DIR"] + urdf_path = locator.locateResource(f"{url}.urdf").getFilePath() + srdf_path = locator.locateResource(f"{url}.srdf").getFilePath() + + urdf_path_str = FilesystemPath(urdf_path) + srdf_path_str = FilesystemPath(srdf_path) + + assert env.init(urdf_path_str, srdf_path_str, locator) + manip_info = ManipulatorInfo() + manip_info.tcp_frame = "tool0" + manip_info.manipulator = "manipulator" + manip_info.working_frame = "base_link" + joint_names = list(env.getJointGroup("manipulator").getJointNames()) + + return env, manip_info, joint_names + + +def print_joints(results): + # Display the output + # Print out the resulting waypoints + for instr in results: + assert instr.isMoveInstruction() + move_instr1 = InstructionPoly_as_MoveInstructionPoly(instr) + wp1 = move_instr1.getWaypoint() + assert wp1.isStateWaypoint() + wp = WaypointPoly_as_StateWaypointPoly(wp1) + print(f"Joint Positions: {wp.getPosition().flatten()} time: {wp.getTime()}") + From 1bafafc02a3c627a83ae11410e41ef573b1ee42a Mon Sep 17 00:00:00 2001 From: Jelle Feringa Date: Tue, 19 Sep 2023 15:47:21 +0200 Subject: [PATCH 2/4] porting the puzzle example from C++ https://github.com/tesseract-robotics/tesseract_planning/blob/master/tesseract_examples/src/glass_upright_example.cpp --- examples/puzzle.py | 2 ++ examples/utils.py | 28 ++++++++++++++++++++-------- 2 files changed, 22 insertions(+), 8 deletions(-) diff --git a/examples/puzzle.py b/examples/puzzle.py index d17bda42..bda3e1e2 100644 --- a/examples/puzzle.py +++ b/examples/puzzle.py @@ -1,6 +1,8 @@ import time import os, sys +# TODO +# Seems to me that a `Vector3d` class isn't available from tesseract? from compas.geometry import Vector # this example is transliterated from C++, you'll find the original here: diff --git a/examples/utils.py b/examples/utils.py index 9c6887c5..fca4630e 100644 --- a/examples/utils.py +++ b/examples/utils.py @@ -1,22 +1,31 @@ import os -from tesseract_robotics.tesseract_command_language import InstructionPoly_as_MoveInstructionPoly, \ - WaypointPoly_as_StateWaypointPoly -from tesseract_robotics.tesseract_common import FilesystemPath, ManipulatorInfo, GeneralResourceLocator +from tesseract_robotics.tesseract_command_language import ( + InstructionPoly_as_MoveInstructionPoly, + WaypointPoly_as_StateWaypointPoly, +) +from tesseract_robotics.tesseract_common import ( + FilesystemPath, + ManipulatorInfo, + GeneralResourceLocator, +) from tesseract_robotics.tesseract_environment import Environment -TESSERACT_SUPPORT_DIR = os.environ['TESSERACT_RESOURCE_PATH'] -TESSERACT_TASK_COMPOSER_DIR = os.environ['TESSERACT_TASK_COMPOSER_CONFIG_FILE'] +TESSERACT_SUPPORT_DIR = os.environ["TESSERACT_RESOURCE_PATH"] +TESSERACT_TASK_COMPOSER_DIR = os.environ["TESSERACT_TASK_COMPOSER_CONFIG_FILE"] task_composer_filename = os.environ["TESSERACT_TASK_COMPOSER_CONFIG_FILE"] + 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" ) return config_path + def support_dir(pth): return FilesystemPath(os.path.join(TESSERACT_SUPPORT_DIR, pth)) @@ -25,10 +34,14 @@ def compose_dir(pth): return FilesystemPath(os.path.join(TESSERACT_TASK_COMPOSER_DIR, pth)) -def get_environment(url) -> tuple[Environment, ManipulatorInfo, list]: +def get_environment(url) -> tuple[Environment, ManipulatorInfo, list[str]]: + """ + given a `url` load a URDF & SRDF and return an Enviornment and Manipulator instance and a + list of joint names + """ locator = GeneralResourceLocator() env = Environment() - #tesseract_support = os.environ["TESSERACT_SUPPORT_DIR"] + # tesseract_support = os.environ["TESSERACT_SUPPORT_DIR"] urdf_path = locator.locateResource(f"{url}.urdf").getFilePath() srdf_path = locator.locateResource(f"{url}.srdf").getFilePath() @@ -55,4 +68,3 @@ def print_joints(results): assert wp1.isStateWaypoint() wp = WaypointPoly_as_StateWaypointPoly(wp1) print(f"Joint Positions: {wp.getPosition().flatten()} time: {wp.getTime()}") - From 4be11c2107c56f670fbcdf51ff5d7a6bd74fcb52 Mon Sep 17 00:00:00 2001 From: Jelle Feringa Date: Thu, 21 Sep 2023 18:46:43 +0200 Subject: [PATCH 3/4] porting the puzzle example from C++ https://github.com/tesseract-robotics/tesseract_planning/blob/master/tesseract_examples/src/glass_upright_example.cpp --- examples/glass_upright_example.py | 305 ++++++++++++++++++ examples/puzzle.py | 20 +- .../tesseract_planning_example_composer.py | 125 +++++-- examples/utils.py | 26 +- 4 files changed, 428 insertions(+), 48 deletions(-) create mode 100644 examples/glass_upright_example.py diff --git a/examples/glass_upright_example.py b/examples/glass_upright_example.py new file mode 100644 index 00000000..3a214491 --- /dev/null +++ b/examples/glass_upright_example.py @@ -0,0 +1,305 @@ +# ported from file glass_upright_example.cpp +# for the original see: +# https://github.com/tesseract-robotics/tesseract_planning/blob/master/tesseract_examples/src/glass_upright_example.cpp + +import time + +import numpy as np +from tesseract_robotics import tesseract_environment as te +from tesseract_robotics import tesseract_geometry as tg +from tesseract_robotics import tesseract_scene_graph as tsg +from tesseract_robotics.tesseract_command_language import ( + CompositeInstruction, + MoveInstructionType_LINEAR, + MoveInstruction, + ProfileDictionary, + CompositeInstructionOrder_ORDERED, + StateWaypoint, + StateWaypointPoly_wrap_StateWaypoint, + MoveInstructionPoly_wrap_MoveInstruction, + AnyPoly_wrap_CompositeInstruction, + AnyPoly_as_CompositeInstruction, + InstructionPoly_as_MoveInstructionPoly, + WaypointPoly_as_StateWaypointPoly, + toJointTrajectory, +) +from tesseract_robotics.tesseract_common import ( + Isometry3d, + ManipulatorInfo, + AnyPoly, + JointTrajectory, +) +from tesseract_robotics.tesseract_environment import Environment +from tesseract_robotics.tesseract_motion_planners_trajopt import ( + ProfileDictionary_addProfile_TrajOptCompositeProfile, + TrajOptDefaultCompositeProfile, + TrajOptDefaultPlanProfile, + ProfileDictionary_addProfile_TrajOptPlanProfile, +) +from tesseract_robotics.tesseract_task_composer import ( + TaskComposerPluginFactory, + TaskComposerDataStorage, + PlanningTaskComposerProblemUPtr, + PlanningTaskComposerProblemUPtr_as_TaskComposerProblemUPtr, + TaskComposerInput, +) + +from examples.puzzle import TRAJOPT_DEFAULT_NAMESPACE +from examples.utils import ( + get_environment, + tesseract_task_composer_config_file, + as_joint_trajectory, +) +from tesseract_viewer_python.tesseract_robotics_viewer import TesseractViewer + + +class GlassUprightExample: + def __init__(self, env: Environment, manip_info: ManipulatorInfo, debug: bool): + self.env = env + self.manip_info = manip_info + + self.manip_info.manipulator = "manipulator" + self.manip_info.tcp_frame = "tool0" + self.manip_info.working_frame = "base_link" + + self.debug = debug + self._initial_joint_state() + self.update_env() + + def addSphere(self) -> te.AddLinkCommand: + link_sphere = tsg.Link("sphere_attached") + visual = tsg.Visual() + origin = Isometry3d.Identity() + + ll = np.array([0.5, 0, 0.55]) + # TODO: stange that a `Translation3d` is not an accepted argument + origin.setTranslation(ll) + + sphere = tg.Sphere(0.15) + visual.geometry = sphere + link_sphere.visual.push_back(visual) + + collision = tsg.Collision() + collision.origin = visual.origin + collision.geometry = visual.geometry + link_sphere.collision.push_back(collision) + + joint_sphere = tsg.Joint("joint_sphere_attached") + joint_sphere.parent_link_name = "base_link" + joint_sphere.child_link_name = link_sphere.getName() + joint_sphere.type = tsg.JointType_FIXED + + cmd = te.AddLinkCommand(link_sphere, joint_sphere) + self.env.applyCommand(cmd) + + return cmd + + def _initial_joint_state(self): + # // Set the robot initial state + self.joint_names = [] + self.joint_names.append("joint_a3") + self.joint_names.append("joint_a4") + self.joint_names.append("joint_a5") + self.joint_names.append("joint_a2") + self.joint_names.append("joint_a1") + self.joint_names.append("joint_a6") + self.joint_names.append("joint_a7") + + self.joint_start_pos = np.zeros((7,)) + self.joint_end_pos = np.zeros((7,)) + + self.joint_start_pos[0] = -0.4 + self.joint_start_pos[1] = 0.2762 + self.joint_start_pos[2] = 0.0 + self.joint_start_pos[3] = -1.3348 + self.joint_start_pos[4] = 0.0 + self.joint_start_pos[5] = 1.4959 + self.joint_start_pos[6] = 0.0 + # + # Eigen::VectorXd self.joint_end_pos(7); + self.joint_end_pos[0] = 0.4 + self.joint_end_pos[1] = 0.2762 + self.joint_end_pos[2] = 0.0 + self.joint_end_pos[3] = -1.3348 + self.joint_end_pos[4] = 0.0 + self.joint_end_pos[5] = 1.4959 + self.joint_end_pos[6] = 0.0 + + self.env.setState(self.joint_names, self.joint_start_pos) + + def update_env(self): + # Get the state solver. This must be called again after environment is updated + solver = self.env.getStateSolver() + + # Get the discrete contact manager. This must be called again after the environment is updated + manager = self.env.getDiscreteContactManager() + manager.setActiveCollisionObjects(self.env.getActiveLinkNames()) + + def plan(self): + self._initial_joint_state() + self.update_env() + cmd = self.addSphere() + self.update_env() + + # Create the input command program. Note the use of *_wrap_MoveInstruction functions. This is required because the + # Python bindings do not support implicit conversion from the MoveInstruction to the MoveInstructionPoly. + program = CompositeInstruction("UPRIGHT", CompositeInstructionOrder_ORDERED) + program.setManipulatorInfo(self.manip_info) + + waypointA = StateWaypoint(self.joint_names, self.joint_start_pos) + waypointB = StateWaypoint(self.joint_names, self.joint_end_pos) + wp0 = StateWaypointPoly_wrap_StateWaypoint(waypointA) + wp1 = StateWaypointPoly_wrap_StateWaypoint(waypointB) + + start_instruction = MoveInstruction(wp0, MoveInstructionType_LINEAR, "UPRIGHT") + start_instruction.setDescription("start instruction") + + plan_f0 = MoveInstruction(wp1, MoveInstructionType_LINEAR, "UPRIGHT") + plan_f0.setDescription("freespace_plan") + + program.appendMoveInstruction( + MoveInstructionPoly_wrap_MoveInstruction(start_instruction) + ) + program.appendMoveInstruction(MoveInstructionPoly_wrap_MoveInstruction(plan_f0)) + + self.profile = ProfileDictionary() + + # todo: no ifopt + composite_profile = TrajOptDefaultCompositeProfile() + + composite_profile.collision_cost_config.enabled = True + # todo: expected tc.CollisionEvaluatorType_DISCRETE_CONTINUOUS + # composite_profile.collision_cost_config.type = tc.CollisionEvaluatorType_CONTINUOUS + composite_profile.collision_cost_config.safety_margin = 0.01 + composite_profile.collision_cost_config.safety_margin_buffer = 0.01 + composite_profile.collision_cost_config.coeff = 1 + composite_profile.collision_constraint_config.enabled = True + # todo: expected tc.CollisionEvaluatorType_DISCRETE_CONTINUOUS + # composite_profile.collision_constraint_config.type = tc.CollisionEvaluatorType_CONTINUOUS + composite_profile.collision_constraint_config.safety_margin = 0.01 + composite_profile.collision_constraint_config.safety_margin_buffer = 0.01 + composite_profile.collision_constraint_config.coeff = 1 + composite_profile.smooth_velocities = True + composite_profile.smooth_accelerations = False + composite_profile.smooth_jerks = False + composite_profile.velocity_coeff = np.array([1.0]) + + # TODO: add a method `add_profile` that add the correct profile + # by inspecting composite_profile.__class__ to have something a little more + # pythonic + + ProfileDictionary_addProfile_TrajOptCompositeProfile( + self.profile, TRAJOPT_DEFAULT_NAMESPACE, "UPRIGHT", composite_profile + ) + + plan_profile = TrajOptDefaultPlanProfile() + plan_profile.joint_coeff = np.ones((7,)) + plan_profile.cartesian_coeff = np.array( + [0.0, 0.0, 0.0, 5.0, 5.0, 5.0], + ) + + ProfileDictionary_addProfile_TrajOptPlanProfile( + self.profile, TRAJOPT_DEFAULT_NAMESPACE, "UPRIGHT", plan_profile + ) + + # TODO: refactor to use the planner class + + fs_pth = tesseract_task_composer_config_file() + self.factory = TaskComposerPluginFactory(fs_pth) + + self.task = self.factory.createTaskComposerNode("TrajOptPipeline") + self.input_key = self.task.getInputKeys()[0] + self.output_key = self.task.getOutputKeys()[0] + + program_anypoly = AnyPoly_wrap_CompositeInstruction(program) + + self.task_data = TaskComposerDataStorage() + self.task_data.setData(self.input_key, program_anypoly) + + self.task_planning_problem = PlanningTaskComposerProblemUPtr.make_unique( + self.env, self.task_data, self.profile + ) + + self.task_problem = PlanningTaskComposerProblemUPtr_as_TaskComposerProblemUPtr( + self.task_planning_problem + ) + self.task_input = TaskComposerInput(self.task_problem) + + # Create an executor to run the task + self.task_executor = self.factory.createTaskComposerExecutor("TaskflowExecutor") + + start = time.time() + # Run the task and wait for completion + future = self.task_executor.run(self.task.get(), self.task_input) + future.wait() + stop = time.time() - start + + print(f"was planning aborted? {self.task_input.isAborted()}") + print(f"was planning succesful? {self.task_input.isSuccessful()}") + + 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) + + # TODO: move to planner + def _create_viewer(self): + # Create a viewer and set the environment so the results can be displayed later + viewer = TesseractViewer() + viewer.update_environment(self.env, [0, 0, 0]) + + # Set the initial state of the robot + viewer.update_joint_positions( + self.joint_names, np.array([1, -0.2, 0.01, 0.3, -0.5, 1]) + ) + + # Start the viewer + viewer.start_serve_background() + return viewer + + def plot(self): + # Retrieve the output, converting the AnyPoly back to a CompositeInstruction + results = as_joint_trajectory(self.task, self.task_data) + + # Display the output + # Print out the resulting waypoints + for instr in results: + print(f"instr: {instr}") + assert instr.isMoveInstruction() + move_instr1 = InstructionPoly_as_MoveInstructionPoly(instr) + wp1 = move_instr1.getWaypoint() + # assert wp1.isStateWaypoint() + wp = WaypointPoly_as_StateWaypointPoly(wp1) + print(f"Joint Positions: {wp.getPosition().flatten()} time: {wp.getTime()}") + + viewer = self._create_viewer() + + # 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, self.manip_info) + + +def run(): + env, manip_info, joint_names = get_environment( + "package://tesseract_support/urdf/lbr_iiwa_14_r820" + ) + + print(env, manip_info) + + gue = GlassUprightExample(env, manip_info, True) + gue.plan() + gue.plot() + + input("press enter to exit") + + +if __name__ == "__main__": + run() diff --git a/examples/puzzle.py b/examples/puzzle.py index bda3e1e2..1bff82d2 100644 --- a/examples/puzzle.py +++ b/examples/puzzle.py @@ -63,7 +63,7 @@ PlanningTaskComposerProblemUPtr_as_TaskComposerProblemUPtr, ) -from utils import get_environment, tesseract_task_composer_config_file +from utils import get_environment, tesseract_task_composer_config_file, as_joint_trajectory TRAJOPT_DEFAULT_NAMESPACE = "TrajOptMotionPlannerTask" @@ -198,7 +198,7 @@ def plan(self) -> AnyPoly_as_CompositeInstruction: start = time.time() # Run the task and wait for completion - future = self.task_executor.run(self.task.get(), self.task_input) + future = self.task_executor.plan(self.task.get(), self.task_input) future.wait() stop = time.time() - start @@ -217,18 +217,6 @@ def plan(self) -> AnyPoly_as_CompositeInstruction: return results -def as_joint_trajectory(planner: Planner) -> JointTrajectory: - # Plot Process Trajectory - # TODO as composite instruction - _ci: AnyPoly = planner.input_data.getData(planner.output_key) - - ci = AnyPoly_as_CompositeInstruction(_ci) - - trajectory: JointTrajectory = toJointTrajectory(ci) - state_solver = planner.t_env.getStateSolver() - return trajectory - - def make_puzzle_tool_poses() -> list[Isometry3d]: path = [] # results locator = GeneralResourceLocator() @@ -257,7 +245,7 @@ def make_puzzle_tool_poses() -> list[Isometry3d]: norm.unitize() - temp_x = (Vector(-1, -1, -1) * pos).unitized() + temp_x = (pos*-1).unitized() y_axis = norm.cross(temp_x).unitized() x_axis = y_axis.cross(norm).unitized() @@ -324,7 +312,7 @@ def run(self): pl.create_task() results = pl.plan() - joint_trajectory = as_joint_trajectory(pl) + joint_trajectory = as_joint_trajectory(pl.task, pl.task_data) # print("Final trajectory is collision-free") # return input.isSuccessful() diff --git a/examples/tesseract_planning_example_composer.py b/examples/tesseract_planning_example_composer.py index 8a5db621..d15d5aef 100644 --- a/examples/tesseract_planning_example_composer.py +++ b/examples/tesseract_planning_example_composer.py @@ -6,19 +6,50 @@ from tesseract_robotics.tesseract_common import GeneralResourceLocator from tesseract_robotics.tesseract_environment import Environment -from tesseract_robotics.tesseract_common import FilesystemPath, Isometry3d, Translation3d, Quaterniond, \ - ManipulatorInfo, AnyPoly, AnyPoly_wrap_double -from tesseract_robotics.tesseract_command_language import CartesianWaypoint, WaypointPoly, \ - MoveInstructionType_FREESPACE, MoveInstruction, InstructionPoly, StateWaypoint, StateWaypointPoly, \ - CompositeInstruction, MoveInstructionPoly, CartesianWaypointPoly, ProfileDictionary, \ - AnyPoly_as_CompositeInstruction, CompositeInstructionOrder_ORDERED, DEFAULT_PROFILE_KEY, \ - 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 - -from tesseract_robotics.tesseract_task_composer import TaskComposerPluginFactory, PlanningTaskComposerProblemUPtr, \ - TaskComposerDataStorage, TaskComposerInput, TaskComposerProblemUPtr, PlanningTaskComposerProblemUPtr_as_TaskComposerProblemUPtr +from tesseract_robotics.tesseract_common import ( + FilesystemPath, + Isometry3d, + Translation3d, + Quaterniond, + ManipulatorInfo, + AnyPoly, + AnyPoly_wrap_double, +) +from tesseract_robotics.tesseract_command_language import ( + CartesianWaypoint, + WaypointPoly, + MoveInstructionType_FREESPACE, + MoveInstruction, + InstructionPoly, + StateWaypoint, + StateWaypointPoly, + CompositeInstruction, + MoveInstructionPoly, + CartesianWaypointPoly, + ProfileDictionary, + AnyPoly_as_CompositeInstruction, + CompositeInstructionOrder_ORDERED, + DEFAULT_PROFILE_KEY, + 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, +) + +from tesseract_robotics.tesseract_task_composer import ( + TaskComposerPluginFactory, + PlanningTaskComposerProblemUPtr, + TaskComposerDataStorage, + TaskComposerInput, + TaskComposerProblemUPtr, + PlanningTaskComposerProblemUPtr_as_TaskComposerProblemUPtr, +) from tesseract_robotics_viewer import TesseractViewer @@ -28,7 +59,7 @@ # runs a sequence of planning steps to generate an output plan with minimal configuration. "Profiles" are used to # configure the planning steps. Profiles are a dictionary of key value pairs that are used to configure the planning # steps. The various planners have default configurations that should work for most use cases. There are numerous -# configurations available for the task composer that execute different sequences of planning steps. This example +# configurations available for the task composer that execute different sequences of planning steps. This example # demonstrates using the "freespace" planner, which is for moving the robot to a desired pose in free space while # avoiding collisions. The freespace planner first uses OMPL to find a collision free path, and then uses TrajOpt # to refine the path. Finally, the TimeOptimalTrajectoryGeneration time parametrization algorithm is used to generate @@ -77,8 +108,12 @@ locator = GeneralResourceLocator() abb_irb2400_urdf_package_url = "package://tesseract_support/urdf/abb_irb2400.urdf" abb_irb2400_srdf_package_url = "package://tesseract_support/urdf/abb_irb2400.srdf" -abb_irb2400_urdf_fname = FilesystemPath(locator.locateResource(abb_irb2400_urdf_package_url).getFilePath()) -abb_irb2400_srdf_fname = FilesystemPath(locator.locateResource(abb_irb2400_srdf_package_url).getFilePath()) +abb_irb2400_urdf_fname = FilesystemPath( + locator.locateResource(abb_irb2400_urdf_package_url).getFilePath() +) +abb_irb2400_srdf_fname = FilesystemPath( + locator.locateResource(abb_irb2400_srdf_package_url).getFilePath() +) t_env = Environment() @@ -94,35 +129,61 @@ # Create a viewer and set the environment so the results can be displayed later viewer = TesseractViewer() -viewer.update_environment(t_env, [0,0,0]) +viewer.update_environment(t_env, [0, 0, 0]) # Set the initial state of the robot -joint_names = ["joint_%d" % (i+1) for i in range(6)] -viewer.update_joint_positions(joint_names, np.array([1,-.2,.01,.3,-.5,1])) +joint_names = ["joint_%d" % (i + 1) for i in range(6)] +viewer.update_joint_positions(joint_names, np.array([1, -0.2, 0.01, 0.3, -0.5, 1])) # Start the viewer viewer.start_serve_background() # Set the initial state of the robot -t_env.setState(joint_names, np.ones(6)*0.1) +t_env.setState(joint_names, np.ones(6) * 0.1) # Create the input command program waypoints -wp1 = CartesianWaypoint(Isometry3d.Identity() * Translation3d(0.8,-0.3,1.455) * Quaterniond(0.70710678,0,0.70710678,0)) -wp2 = CartesianWaypoint(Isometry3d.Identity() * Translation3d(0.8,0.3,1.455) * Quaterniond(0.70710678,0,0.70710678,0)) -wp3 = CartesianWaypoint(Isometry3d.Identity() * Translation3d(0.8,0.5,1.455) * Quaterniond(0.70710678,0,0.70710678,0)) +wp1 = CartesianWaypoint( + Isometry3d.Identity() + * Translation3d(0.8, -0.3, 1.455) + * Quaterniond(0.70710678, 0, 0.70710678, 0) +) +wp2 = CartesianWaypoint( + Isometry3d.Identity() + * Translation3d(0.8, 0.3, 1.455) + * Quaterniond(0.70710678, 0, 0.70710678, 0) +) +wp3 = CartesianWaypoint( + Isometry3d.Identity() + * Translation3d(0.8, 0.5, 1.455) + * Quaterniond(0.70710678, 0, 0.70710678, 0) +) # Create the input command program instructions. Note the use of explicit construction of the CartesianWaypointPoly # using the *_wrap_CartesianWaypoint functions. This is required because the Python bindings do not support implicit # conversion from the CartesianWaypoint to the CartesianWaypointPoly. -start_instruction = MoveInstruction(CartesianWaypointPoly_wrap_CartesianWaypoint(wp1), MoveInstructionType_FREESPACE, "DEFAULT") -plan_f1 = MoveInstruction(CartesianWaypointPoly_wrap_CartesianWaypoint(wp2), MoveInstructionType_FREESPACE, "DEFAULT") -plan_f2 = MoveInstruction(CartesianWaypointPoly_wrap_CartesianWaypoint(wp3), MoveInstructionType_FREESPACE, "DEFAULT") +start_instruction = MoveInstruction( + CartesianWaypointPoly_wrap_CartesianWaypoint(wp1), + MoveInstructionType_FREESPACE, + "DEFAULT", +) +plan_f1 = MoveInstruction( + CartesianWaypointPoly_wrap_CartesianWaypoint(wp2), + MoveInstructionType_FREESPACE, + "DEFAULT", +) +plan_f2 = MoveInstruction( + CartesianWaypointPoly_wrap_CartesianWaypoint(wp3), + MoveInstructionType_FREESPACE, + "DEFAULT", +) # Create the input command program. Note the use of *_wrap_MoveInstruction functions. This is required because the # Python bindings do not support implicit conversion from the MoveInstruction to the MoveInstructionPoly. program = CompositeInstruction("DEFAULT") program.setManipulatorInfo(manip_info) -program.appendMoveInstruction(MoveInstructionPoly_wrap_MoveInstruction(start_instruction)) +program.appendMoveInstruction( + MoveInstructionPoly_wrap_MoveInstruction(start_instruction) +) program.appendMoveInstruction(MoveInstructionPoly_wrap_MoveInstruction(plan_f1)) # program.appendMoveInstruction(MoveInstructionPoly(plan_f2)) @@ -150,8 +211,12 @@ task_data.setData(input_key, program_anypoly) # Create the task problem and input -task_planning_problem = PlanningTaskComposerProblemUPtr.make_unique(t_env, task_data, profiles) -task_problem = PlanningTaskComposerProblemUPtr_as_TaskComposerProblemUPtr(task_planning_problem) +task_planning_problem = PlanningTaskComposerProblemUPtr.make_unique( + t_env, task_data, profiles +) +task_problem = PlanningTaskComposerProblemUPtr_as_TaskComposerProblemUPtr( + task_planning_problem +) task_input = TaskComposerInput(task_problem) # Create an executor to run the task @@ -179,4 +244,4 @@ viewer.update_trajectory(results) viewer.plot_trajectory(results, manip_info) -input("press enter to exit") \ No newline at end of file +input("press enter to exit") diff --git a/examples/utils.py b/examples/utils.py index fca4630e..668ea98a 100644 --- a/examples/utils.py +++ b/examples/utils.py @@ -1,15 +1,17 @@ import os +import numpy as np from tesseract_robotics.tesseract_command_language import ( InstructionPoly_as_MoveInstructionPoly, - WaypointPoly_as_StateWaypointPoly, + WaypointPoly_as_StateWaypointPoly, AnyPoly_as_CompositeInstruction, toJointTrajectory, ) from tesseract_robotics.tesseract_common import ( FilesystemPath, ManipulatorInfo, - GeneralResourceLocator, + GeneralResourceLocator, Isometry3d, JointTrajectory, AnyPoly, ) from tesseract_robotics.tesseract_environment import Environment +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"] @@ -68,3 +70,23 @@ def print_joints(results): assert wp1.isStateWaypoint() wp = WaypointPoly_as_StateWaypointPoly(wp1) print(f"Joint Positions: {wp.getPosition().flatten()} time: {wp.getTime()}") + + +def _translation(p) -> Isometry3d: + H = np.eye(4) + H[0:3,3] = p + return Isometry3d(H) + + +def as_joint_trajectory(task: TaskComposerNode, tcds: TaskComposerDataStorage) -> JointTrajectory: + # Plot Process Trajectory + # TODO as composite instruction + + output_key = task.getOutputKeys()[0] + _ci: AnyPoly = tcds.getData(output_key) + + ci = AnyPoly_as_CompositeInstruction(_ci) + + trajectory: JointTrajectory = toJointTrajectory(ci) + return trajectory + From 52051e9707618c487d7e3491eec5d03ab11a7a3f Mon Sep 17 00:00:00 2001 From: jelle Date: Fri, 22 Sep 2023 12:40:37 +0200 Subject: [PATCH 4/4] brush up before submitting draft PR --- examples/glass_upright_example.py | 15 ++--- .../{puzzle.py => puzzle_piece_example.py} | 65 +++++++++---------- examples/utils.py | 26 +++++--- 3 files changed, 51 insertions(+), 55 deletions(-) rename examples/{puzzle.py => puzzle_piece_example.py} (88%) diff --git a/examples/glass_upright_example.py b/examples/glass_upright_example.py index 3a214491..2de5fdc2 100644 --- a/examples/glass_upright_example.py +++ b/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") diff --git a/examples/puzzle.py b/examples/puzzle_piece_example.py similarity index 88% rename from examples/puzzle.py rename to examples/puzzle_piece_example.py index 1bff82d2..40948515 100644 --- a/examples/puzzle.py +++ b/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 )") diff --git a/examples/utils.py b/examples/utils.py index 668ea98a..3dcf0e0b 100644 --- a/examples/utils.py +++ b/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 -