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.

Sign up for GitHub

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

Don't use MTC in Plan and Save Trajectory objective #82

Merged
merged 1 commit into from
Jan 9, 2025
Merged

Don't use MTC in Plan and Save Trajectory objective #82

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
57 changes: 56 additions & 1 deletion src/lab_sim/objectives/create_pose_vector.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,17 +1,20 @@
<?xml version="1.0" encoding="utf-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Create Pose Vector">
<!--//////////-->
<BehaviorTree
ID="Create Pose Vector"
_favorite="false"
_subtreeOnly="true"
_description=""
target_poses="{target_poses}"
>
<Control ID="Sequence">
<Action
ID="CreateStampedPose"
orientation_xyzw="0;-1;0;0"
position_xyz="1;0.5;1"
position_xyz="0.75;0.5;0.9"
stamped_pose="{stamped_pose}"
reference_frame="world"
/>
<Action
ID="AddPoseStampedToVector"
Expand All @@ -23,6 +26,31 @@
orientation_xyzw="0;-1;0;0"
position_xyz="0.5;0.5;0.8"
stamped_pose="{stamped_pose}"
reference_frame="world"
/>
<Action
ID="AddPoseStampedToVector"
vector="{target_poses}"
input="{stamped_pose}"
/>
<Action
ID="CreateStampedPose"
orientation_xyzw="0;-1;0;0"
position_xyz="0.25;0.5;0.7"
stamped_pose="{stamped_pose}"
reference_frame="world"
/>
<Action
ID="AddPoseStampedToVector"
vector="{target_poses}"
input="{stamped_pose}"
/>
<Action
ID="CreateStampedPose"
orientation_xyzw="0;-1;0;0"
position_xyz="0;0.5;0.6"
stamped_pose="{stamped_pose}"
reference_frame="world"
/>
<Action
ID="AddPoseStampedToVector"
Expand All @@ -34,6 +62,31 @@
orientation_xyzw="0;-1;0;0"
position_xyz="-0.25;0.5;0.7"
stamped_pose="{stamped_pose}"
reference_frame="world"
/>
<Action
ID="AddPoseStampedToVector"
vector="{target_poses}"
input="{stamped_pose}"
/>
<Action
ID="CreateStampedPose"
orientation_xyzw="0;-1;0;0"
position_xyz="-0.5;0.5;0.8"
stamped_pose="{stamped_pose}"
reference_frame="world"
/>
<Action
ID="AddPoseStampedToVector"
vector="{target_poses}"
input="{stamped_pose}"
/>
<Action
ID="CreateStampedPose"
orientation_xyzw="0;-1;0;0"
position_xyz="-0.75;0.5;0.9"
stamped_pose="{stamped_pose}"
reference_frame="world"
/>
<Action
ID="AddPoseStampedToVector"
Expand All @@ -45,6 +98,7 @@
orientation_xyzw="0;-1;0;0"
position_xyz="-1;0.5;1"
stamped_pose="{stamped_pose}"
reference_frame="world"
/>
<Action
ID="AddPoseStampedToVector"
Expand All @@ -58,6 +112,7 @@
<output_port name="target_poses" default="{target_poses}" />
<MetadataFields>
<Metadata subcategory="Application - Basic Examples" />
<Metadata runnable="false" />
</MetadataFields>
</SubTree>
</TreeNodesModel>
Expand Down
34 changes: 32 additions & 2 deletions src/lab_sim/objectives/load_and_execute_joint_trajectory.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
<?xml version="1.0" encoding="utf-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Load and Execute Joint Trajectory">
<!--//////////-->
<BehaviorTree
Expand All @@ -7,26 +8,55 @@
_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 -->
<!--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"
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"
/>
<Action
ID="LoadJointTrajectoryFromYaml"
file_path="joint_trajectory.yaml"
output="{joint_trajectory_msg}"
/>
<!--Validate the loaded trajectory with the current planning scene to check for collisions before executing-->
<Action
ID="GetCurrentPlanningScene"
planning_scene_msg="{planning_scene}"
/>
<Action
ID="ValidateTrajectory"
cartesian_space_step="0.020000"
joint_space_step="0.100000"
joint_trajectory_msg="{joint_trajectory_msg}"
planning_group_name="manipulator"
planning_scene_msg="{planning_scene}"
debug_solution="{debug_solution}"
/>
<Action
ID="ExecuteFollowJointTrajectory"
execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
goal_duration_tolerance="-1.000000"
goal_position_tolerance="0.000000"
goal_time_tolerance="0.000000"
joint_trajectory_msg="{joint_trajectory_msg}"
/>
<Action ID="ExecuteFollowJointTrajectory" />
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Load and Execute Joint Trajectory">
<MetadataFields>
<Metadata subcategory="Application - Basic Examples" />
<Metadata runnable="true" />
</MetadataFields>
</SubTree>
</TreeNodesModel>
Expand Down
51 changes: 18 additions & 33 deletions src/lab_sim/objectives/plan_and_save_trajectory.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@
_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 -->
<!--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"
Expand All @@ -23,45 +23,30 @@
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 -->
<!--Create a vector of poses we want to move through-->
<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"
target_poses="{target_poses}"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
planning_group_name="manipulator"
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"
mtc_task="{mtc_task}"
waypoint_names="Pick Cube;Place Cube"
joint_group_name="manipulator"
planner_interface="moveit_default"
/>
<!-- Perform the computationally intensive calculation now -->
<Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}" />
<!-- 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 -->
<!--Plan a cartesian path through the desired poses-->
<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"
ID="PlanCartesianPath"
acceleration_scale_factor="1.000000"
blending_radius="0.020000"
ik_cartesian_space_density="0.010000"
ik_joint_space_density="0.100000"
path="{target_poses}"
planning_group_name="manipulator"
position_only="false"
tip_link="grasp_link"
trajectory_sampling_rate="100"
velocity_scale_factor="1.000000"
debug_solution="{debug_solution}"
joint_trajectory_msg="{joint_trajectory_msg}"
/>
<!--Preview the trajectory for approval and save if approved-->
<Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}" />
<Action
ID="SaveJointTrajectoryToYaml"
yaml_filename="~/user_ws/src/lab_sim/objectives/joint_trajectory"
Expand Down
0 ...ve_and_execute_a_trajectory_from_disk.xml → ...ves/record_and_replay_scanning_motion.xml
View file
Open in desktop
File renamed without changes.
Loading