Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Updating objectives with comments for the record and playback objective #71

Merged
merged 1 commit into from
Jan 2, 2025
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 7 additions & 3 deletions src/lab_sim/objectives/add_waypoints_to_mtc_task.xml
Original file line number Diff line number Diff line change
@@ -5,12 +5,15 @@
ID="Add Waypoints to MTC Task"
_subtreeOnly="true"
_description=""
joint_group_name="manipulator"
planner_interface="moveit_default"
mtc_task="{mtc_task}"
>
<Control ID="Sequence">
<Action
ID="RetrieveWaypoint"
waypoint_joint_state="{target_joint_state}"
waypoint_name="Above Pick Cube"
waypoint_name="Look at Table"
joint_group_name="{joint_group_name}"
/>
<Action
@@ -24,7 +27,7 @@
<Action
ID="RetrieveWaypoint"
waypoint_joint_state="{target_joint_state}"
waypoint_name="Above Place Cube"
waypoint_name="Workspace Right"
joint_group_name="{joint_group_name}"
/>
<Action
@@ -40,10 +43,11 @@
<TreeNodesModel>
<SubTree ID="Add Waypoints to MTC Task">
<input_port name="joint_group_name" default="manipulator" />
<inout_port name="mtc_task" default="{mtc_task}" />
<input_port name="planner_interface" default="moveit_default" />
<inout_port name="mtc_task" default="{mtc_task}" />
<MetadataFields>
<Metadata subcategory="Application - Basic Examples" />
<Metadata runnable="false" />
</MetadataFields>
</SubTree>
</TreeNodesModel>
55,924 changes: 19,688 additions & 36,236 deletions src/lab_sim/objectives/joint_trajectory.yaml

Large diffs are not rendered by default.

Original file line number Diff line number Diff line change
@@ -7,6 +7,7 @@
_subtreeOnly="false"
>
<Control ID="Sequence" name="TopLevelSequence">
<!-- Make sure to move the robot to the same starting position as the saved trajectory starts, otherwise the joint_trajectory_controller will use joint interpolation to align the robot with the path -->
<SubTree
ID="Move to Waypoint"
_collapsed="true"
23 changes: 21 additions & 2 deletions src/lab_sim/objectives/plan_and_save_trajectory.xml
Original file line number Diff line number Diff line change
@@ -8,19 +8,28 @@
_subtreeOnly="false"
>
<Control ID="Sequence" name="TopLevelSequence">
<!-- Before we plan we want to make sure we are at a known position, so when we execute the plan it can be done from the same position -->
<SubTree
ID="Move to Waypoint"
_collapsed="true"
waypoint_name="Workspace Right"
joint_group_name="manipulator"
planner_interface="moveit_default"
controller_names="/joint_trajectory_controller"
acceleration_scale_factor="1.0"
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
keep_orientation="false"
keep_orientation_tolerance="0.05"
link_padding="0.01"
velocity_scale_factor="1.0"
/>
<!-- Create a vector of poses so we can add them all to the MTC task at once -->
<SubTree
ID="Create Pose Vector"
_collapsed="false"
target_poses="{target_poses}"
/>
<!-- Add the cartesian poses we created in the previous step -->
<SubTree
ID="Add Poses to MTC Task"
_collapsed="true"
@@ -30,6 +39,7 @@
ik_frame="grasp_link"
mtc_task="{mtc_task}"
/>
<!-- Add joint-state waypoints to the MTC task as well -->
<SubTree
ID="Add Waypoints to MTC Task"
_collapsed="false"
@@ -38,23 +48,32 @@
joint_group_name="manipulator"
planner_interface="moveit_default"
/>
<!-- Perform the computationally intensive calculation now -->
<Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}" />
<Action ID="WaitForUserTrajectoryApproval" />
<!--The following behavior can be found in the example_behaviors package.-->
<!-- Preview the trajectory for approval -->
<Action ID="WaitForUserTrajectoryApproval" solution="{mtc_solution}" />
<!-- The MTC solution does not contain time parameterization, so we can use TOTG to convert it to a JointTrajectory. The source code for the following behavior can be found in the example_behaviors package -->
<Action
ID="ConvertMtcSolutionToJointTrajectory"
joint_trajectory="{joint_trajectory_msg}"
acceleration_scaling_factor="1.000000"
joint_group="manipulator"
sampling_rate="100"
solution="{mtc_solution}"
velocity_scaling_factor="1.000000"
/>
<Action
ID="SaveJointTrajectoryToYaml"
yaml_filename="~/user_ws/src/lab_sim/objectives/joint_trajectory"
message="{joint_trajectory_msg}"
/>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Plan and Save Trajectory">
<MetadataFields>
<Metadata subcategory="Application - Advanced Examples" />
<Metadata runnable="true" />
</MetadataFields>
</SubTree>
</TreeNodesModel>
3 changes: 3 additions & 0 deletions src/lab_sim/objectives/plan_to_pose.xml
Original file line number Diff line number Diff line change
@@ -12,7 +12,9 @@
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
task="{mtc_task}"
/>
<!-- Start the MTC task from the robot's current pose -->
<Action ID="SetupMTCCurrentState" task="{mtc_task}" />
<!-- Use a script with a pose counter so we can give a unique name to each marker (allowing multiple poses to be visualized) -->
<Action ID="Script" code="pose_ctr := 0" />
<Decorator
ID="ForEach"
@@ -21,6 +23,7 @@
index="{index}"
>
<Control ID="Sequence">
<!-- Visualize and add each pose to the vector -->
<Action
ID="VisualizePose"
marker_name="{pose_ctr}"
21 changes: 11 additions & 10 deletions src/lab_sim/objectives/save_and_execute_a_trajectory_from_disk.xml
Original file line number Diff line number Diff line change
@@ -1,23 +1,24 @@
<root
BTCPP_format="4"
main_tree_to_execute="Save and Execute a Trajectory from Disk"
>
<?xml version="1.0" encoding="utf-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Record and Replay Scanning Motion">
<!--//////////-->
<BehaviorTree
ID="Save and Execute a Trajectory from Disk"
_description="Build a trajectory from a series of waypoints and MTC tasks, then save the trajectory to disk. Load the trajectory from disk and execute it as well."
ID="Record and Replay Scanning Motion"
_description="Build a trajectory from a series of waypoints and MTC tasks to scan the environment, then save the trajectory to disk. If the trajectory is approved, it will be saved and then executed."
_favorite="false"
_subtreeOnly="false"
>
<Control ID="Sequence" name="TopLevelSequence">
<SubTree ID="Plan and Save Trajectory" _collapsed="true" />
<SubTree ID="Load and Execute Joint Trajectory" _collapsed="true" />
<!--First we build the trajectory using caresian and joint-state waypoint. If the trajecotry is approved, it will be saved.-->
<SubTree ID="Plan and Save Trajectory" _collapsed="false" />
<!--Next we load the saved trajectory from disk an execute it. This stage does not require motion planning-->
<SubTree ID="Load and Execute Joint Trajectory" _collapsed="false" />
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Save and Execute a Trajectory from Disk">
<SubTree ID="Record and Replay Scanning Motion">
<MetadataFields>
<Metadata subcategory="Motion - Execute" />
<Metadata subcategory="Application - Advanced Examples" />
<Metadata runnable="true" />
</MetadataFields>
</SubTree>
</TreeNodesModel>