Skip to content

Commit

Permalink
Merge pull request #24 from PickNikRobotics/dev-4.0
Browse files Browse the repository at this point in the history 
Dev 4.0
  • Loading branch information
@dyackzan
dyackzan authored Jan 29, 2024
2 parents eef8834 + 5a3ad6f commit 8abd5cd
Show file tree
Hide file tree
Showing 4 changed files with 49 additions and 24 deletions.
21 changes: 14 additions & 7 deletions src/kinova_gen3_base_config/objectives/interpolate_to_joint_state.xml
View file
Original file line number Diff line number Diff line change
@@ -1,13 +1,20 @@
<?xml version="1.0"?>
<root BTCPP_format="4" main_tree_to_execute="Interpolate to Joint State">
<BehaviorTree ID="Interpolate to Joint State" _description="Move to a specified joint state using joint interpolation">
<Control ID="Sequence" name="root">
<Action ID="RetrieveJointStateParameter" timeout_sec="" joint_state="{target_joint_state}"/>
<Action ID="InitializeMTCTask" task_id="interpolate_to_joint_state" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{interpolate_to_waypoint_task}"/>
<Action ID="SetupMTCCurrentState" task="{interpolate_to_waypoint_task}"/>
<Action ID="SetupMTCInterpolateToJointState" joint_state="{target_joint_state}" name="SetupMTCInterpolateToJointState_First" planning_group_name="manipulator" task="{interpolate_to_waypoint_task}"/>
<Action ID="PlanMTCTask" solution="{interpolate_to_waypoint_solution}" task="{interpolate_to_waypoint_task}"/>
<Action ID="ExecuteMTCTask" solution="{interpolate_to_waypoint_solution}"/>
<Control ID="Fallback" name="root">
<Control ID="Sequence">
<Action ID="RetrieveJointStateParameter" timeout_sec="" joint_state="{target_joint_state}"/>
<Action ID="InitializeMTCTask" task_id="interpolate_to_joint_state" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{interpolate_to_waypoint_task}"/>
<Action ID="SetupMTCCurrentState" task="{interpolate_to_waypoint_task}"/>
<Action ID="SetupMTCInterpolateToJointState" joint_state="{target_joint_state}" name="SetupMTCInterpolateToJointState_First" planning_group_name="manipulator" task="{interpolate_to_waypoint_task}"/>
<Action ID="PlanMTCTask" solution="{interpolate_to_waypoint_solution}" task="{interpolate_to_waypoint_task}"/>
<Action ID="ExecuteMTCTask" solution="{interpolate_to_waypoint_solution}"/>
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
</Control>
<Control ID="Sequence">
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
<Action ID="AlwaysFailure"/>
</Control>
</Control>
</BehaviorTree>
</root>
21 changes: 14 additions & 7 deletions src/kinova_gen3_base_config/objectives/move_to_joint_state.xml
View file
Original file line number Diff line number Diff line change
@@ -1,13 +1,20 @@
<?xml version="1.0"?>
<root BTCPP_format="4" main_tree_to_execute="Move to Joint State">
<BehaviorTree ID="Move to Joint State" _description="Move to a specified joint state">
<Control ID="Sequence" name="root">
<Action ID="RetrieveJointStateParameter" timeout_sec="" joint_state="{target_joint_state}"/>
<Action ID="InitializeMTCTask" task_id="move_to_joint_state" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{move_to_waypoint_task}"/>
<Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
<Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState_First" planning_group_name="manipulator" task="{move_to_waypoint_task}" use_all_planners="true" constraints=""/>
<Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
<Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}"/>
<Control ID="Fallback" name="root">
<Control ID="Sequence">
<Action ID="RetrieveJointStateParameter" timeout_sec="" joint_state="{target_joint_state}"/>
<Action ID="InitializeMTCTask" task_id="move_to_joint_state" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{move_to_waypoint_task}"/>
<Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
<Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState_First" planning_group_name="manipulator" task="{move_to_waypoint_task}" use_all_planners="true" constraints=""/>
<Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
<Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}"/>
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
</Control>
<Control ID="Sequence">
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
<Action ID="AlwaysFailure"/>
</Control>
</Control>
</BehaviorTree>
</root>
23 changes: 15 additions & 8 deletions src/kinova_gen3_base_config/objectives/move_to_pose.xml
View file
Original file line number Diff line number Diff line change
@@ -1,14 +1,21 @@
<?xml version="1.0"?>
<root BTCPP_format="4" main_tree_to_execute="Move to Pose">
<BehaviorTree ID="Move to Pose" _description="Uses inverse kinematics to move the robot to a set gripper position">
<Control ID="Sequence">
<Action ID="RetrievePoseParameter" timeout_sec="" pose="{target_pose}"/>
<Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="/joint_trajectory_controller" task="{move_to_pose_task}"/>
<Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
<Action ID="SetupMTCMoveToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}" use_all_planners="false"/>
<Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
<SubTree ID="Wait for Trajectory Approval if User Available" solution="{move_to_pose_solution}"/>
<Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
<Control ID="Fallback" name="root">
<Control ID="Sequence">
<Action ID="RetrievePoseParameter" timeout_sec="" pose="{target_pose}"/>
<Action ID="InitializeMTCTask" task_id="move_to_pose" controller_names="/joint_trajectory_controller" task="{move_to_pose_task}"/>
<Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
<Action ID="SetupMTCMoveToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{move_to_pose_task}" use_all_planners="false"/>
<Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
<SubTree ID="Wait for Trajectory Approval if User Available" solution="{move_to_pose_solution}"/>
<Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
</Control>
<Control ID="Sequence">
<Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>
<Action ID="AlwaysFailure"/>
</Control>
</Control>
</BehaviorTree>
</root>
8 changes: 6 additions & 2 deletions src/kinova_gen3_base_config/objectives/take_snapshot.xml
View file
Original file line number Diff line number Diff line change
@@ -1,11 +1,15 @@
<?xml version="1.0"?>
<root BTCPP_format="4" main_tree_to_execute="Take Snapshot">
<!-- ////////// -->
<BehaviorTree ID="Take Snapshot" _description="Take a point cloud snapshot and add to collision scene">
<BehaviorTree ID="Take Snapshot" _description="Take a point cloud snapshot and add to collision scene" _favorite="true">
<Control ID="Sequence">
<Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" />
<Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/>
<Action ID="GetStringFromUser" parameter_name="take_snapshot.uuid" parameter_value="{uuid}" />
<Control ID="Fallback" name="TryGetStringFromUser">
<Action ID="GetStringFromUser" parameter_name="take_snapshot.uuid" parameter_value="{uuid}" />
<!-- if we fail to get the string from the user fallback to an empty value for the uuid -->
<Action ID="Script" code="uuid := ''"/>
</Control>
<Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" sensor_name="scene_scan_camera" pcd_topic="/pcd_pointcloud_captures" point_cloud_uuid="{uuid}"/>
</Control>
</BehaviorTree>
Expand Down

0 comments on commit 8abd5cd

Please sign in to comment.