Skip to content

Commit

Permalink
formatting
Browse files Browse the repository at this point in the history
  • Loading branch information
@MikeWrock
MikeWrock committed Nov 18, 2024
1 parent b090cff commit 23d20c4
Show file tree
Hide file tree
Showing 4 changed files with 117 additions and 25 deletions.
40 changes: 32 additions & 8 deletions src/lab_sim/objectives/add_waypoints_to_mtc_task.xml
View file
Original file line number Diff line number Diff line change
@@ -1,19 +1,43 @@
<?xml version="1.0" encoding="utf-8"?>
<?xml version="1.0" encoding="utf-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Add Waypoints to MTC Task">
<!--//////////-->
<BehaviorTree ID="Add Waypoints to MTC Task">
<Control ID="Sequence">
<Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="Pick Cube" joint_group_name="{joint_group_name}"/>
<Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState" planning_group_name="{joint_group_name}" planner_interface="{planner_interface}" task="{mtc_task}"/>
<Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="Place Cube" joint_group_name="{joint_group_name}"/>
<Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState" planning_group_name="{joint_group_name}" planner_interface="{planner_interface}" task="{mtc_task}"/>
<Action
ID="RetrieveWaypoint"
waypoint_joint_state="{target_joint_state}"
waypoint_name="Pick Cube"
joint_group_name="{joint_group_name}"
/>
<Action
ID="SetupMTCMoveToJointState"
joint_state="{target_joint_state}"
name="SetupMTCMoveToJointState"
planning_group_name="{joint_group_name}"
planner_interface="{planner_interface}"
task="{mtc_task}"
/>
<Action
ID="RetrieveWaypoint"
waypoint_joint_state="{target_joint_state}"
waypoint_name="Place Cube"
joint_group_name="{joint_group_name}"
/>
<Action
ID="SetupMTCMoveToJointState"
joint_state="{target_joint_state}"
name="SetupMTCMoveToJointState"
planning_group_name="{joint_group_name}"
planner_interface="{planner_interface}"
task="{mtc_task}"
/>
</Control>
</BehaviorTree>
<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"/>
<input_port name="joint_group_name" default="manipulator" />
<inout_port name="mtc_task" default="{mtc_task}" />
<input_port name="planner_interface" default="moveit_default" />
</SubTree>
</TreeNodesModel>
</root>
33 changes: 32 additions & 1 deletion src/lab_sim/objectives/pick_object.xml
View file
Original file line number Diff line number Diff line change
@@ -1 +1,32 @@
<root BTCPP_format="4" main_tree_to_execute="Pick object"><!--//////////--><BehaviorTree ID="Pick object" _description="" _favorite="false" _subtreeOnly="true"><Control ID="Sequence" name="root"><SubTree ID="Open Gripper"/><Control ID="Sequence"><Action ID="SwitchUIPrimaryView" primary_view_name="Visualization"/><Action ID="CreateStampedPose" orientation_xyzw="0;1;0;0" position_xyz="0.01;.75;0.515" stamped_pose="{stamped_pose}"/><Action ID="AddPoseStampedToVector" input="{stamped_pose}" vector="{pose_vector}"/></Control><SubTree ID="Pick first object in vector" pose_vector="{pose_vector}" _collapsed="false"/></Control></BehaviorTree></root>
<root BTCPP_format="4" main_tree_to_execute="Pick object">
<!--//////////-->
<BehaviorTree
ID="Pick object"
_description=""
_favorite="false"
_subtreeOnly="true"
>
<Control ID="Sequence" name="root">
<SubTree ID="Open Gripper" />
<Control ID="Sequence">
<Action ID="SwitchUIPrimaryView" primary_view_name="Visualization" />
<Action
ID="CreateStampedPose"
orientation_xyzw="0;1;0;0"
position_xyz="0.01;.75;0.515"
stamped_pose="{stamped_pose}"
/>
<Action
ID="AddPoseStampedToVector"
input="{stamped_pose}"
vector="{pose_vector}"
/>
</Control>
<SubTree
ID="Pick first object in vector"
pose_vector="{pose_vector}"
_collapsed="false"
/>
</Control>
</BehaviorTree>
</root>
10 changes: 5 additions & 5 deletions src/lab_sim/objectives/pick_up_cube.xml
View file
Original file line number Diff line number Diff line change
@@ -1,13 +1,13 @@
<?xml version="1.0" encoding="utf-8"?>
<?xml version="1.0" encoding="utf-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Pick up Cube">
<!--//////////-->
<BehaviorTree ID="Pick up Cube" _description="" _favorite="false">
<Control ID="Sequence" name="TopLevelSequence">
<SubTree ID="Clear Snapshot" _collapsed="true"/>
<SubTree ID="Look at table" _collapsed="true"/>
<SubTree ID="Clear Snapshot" _collapsed="true" />
<SubTree ID="Look at table" _collapsed="true" />
<!--Take a snapshot so we have a visualization of the cubes to use in grasp pose correction-->
<SubTree ID="Wrist camera snapshot" _collapsed="true"/>
<SubTree ID="Pick object" _collapsed="false"/>
<SubTree ID="Wrist camera snapshot" _collapsed="true" />
<SubTree ID="Pick object" _collapsed="false" />
</Control>
</BehaviorTree>
</root>
59 changes: 48 additions & 11 deletions src/lab_sim/objectives/plan_and_save_trajectory.xml
View file
Original file line number Diff line number Diff line change
@@ -1,19 +1,56 @@
<?xml version="1.0" encoding="utf-8"?>
<?xml version="1.0" encoding="utf-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Plan and Save Trajectory">
<!--//////////-->
<BehaviorTree ID="Plan and Save Trajectory" _description="" _favorite="true" _subtreeOnly="false">
<BehaviorTree
ID="Plan and Save Trajectory"
_description=""
_favorite="true"
_subtreeOnly="false"
>
<Control ID="Sequence" name="TopLevelSequence">
<SubTree ID="Move to Waypoint" _collapsed="true" waypoint_name="Workspace Right" joint_group_name="manipulator" planner_interface="moveit_default" controller_names="/joint_trajectory_controller"/>
<SubTree ID="Create Pose Vector" _collapsed="false" target_poses="{target_poses}"/>
<SubTree ID="Add Poses to MTC Task" _collapsed="true" target_poses="{target_poses}" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" ik_frame="grasp_link" mtc_task="{mtc_task}"/>
<SubTree ID="Add Waypoints to MTC Task" _collapsed="false" mtc_task="{mtc_task}" waypoint_names="Pick Cube;Place Cube" joint_group_name="manipulator" planner_interface="moveit_default"/>
<Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}"/>
<Action ID="WaitForUserTrajectoryApproval"/>
<Action ID="ConvertMtcSolutionToJointTrajectory" joint_trajectory="{joint_trajectory_msg}"/>
<Action ID="SaveJointTrajectoryToYaml" yaml_filename="~/user_ws/src/lab_sim/objectives/joint_trajectory"/>
<SubTree
ID="Move to Waypoint"
_collapsed="true"
waypoint_name="Workspace Right"
joint_group_name="manipulator"
planner_interface="moveit_default"
controller_names="/joint_trajectory_controller"
/>
<SubTree
ID="Create Pose Vector"
_collapsed="false"
target_poses="{target_poses}"
/>
<SubTree
ID="Add Poses to MTC Task"
_collapsed="true"
target_poses="{target_poses}"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
planning_group_name="manipulator"
ik_frame="grasp_link"
mtc_task="{mtc_task}"
/>
<SubTree
ID="Add Waypoints to MTC Task"
_collapsed="false"
mtc_task="{mtc_task}"
waypoint_names="Pick Cube;Place Cube"
joint_group_name="manipulator"
planner_interface="moveit_default"
/>
<Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}" />
<Action ID="WaitForUserTrajectoryApproval" />
<Action
ID="ConvertMtcSolutionToJointTrajectory"
joint_trajectory="{joint_trajectory_msg}"
/>
<Action
ID="SaveJointTrajectoryToYaml"
yaml_filename="~/user_ws/src/lab_sim/objectives/joint_trajectory"
/>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Plan and Save Trajectory"/>
<SubTree ID="Plan and Save Trajectory" />
</TreeNodesModel>
</root>

0 comments on commit 23d20c4

Please sign in to comment.