Skip to content

Commit

Permalink
Merge pull request #77 from PickNikRobotics/update-kinova-objectives-7.0
Browse files Browse the repository at this point in the history 
Update kinova objectives
  • Loading branch information
@MikeWrock
MikeWrock authored Jan 18, 2025
2 parents 13d0f1a + a48d1f0 commit bcd5add
Show file tree
Hide file tree
Showing 7 changed files with 293 additions and 53 deletions.
28 changes: 18 additions & 10 deletions src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_along_path.xml
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Move Along Path">
<!-- ////////// -->
<!--//////////-->
<BehaviorTree
ID="Move Along Path"
_description="Moves the robot tip on a rectangular path."
Expand All @@ -9,6 +9,10 @@
>
<Control ID="Sequence" name="TopLevelSequence">
<Control ID="Sequence">
<Action
ID="ActivateControllers"
controller_names="/joint_trajectory_controller"
/>
<Action
ID="CreateStampedPose"
reference_frame="base_link"
Expand All @@ -18,8 +22,8 @@
/>
<Action
ID="AddPoseStampedToVector"
input_pose="{stamped_pose}"
pose_stamped_vector="{pose_stamped_vector}"
input="{stamped_pose}"
vector="{pose_stamped_vector}"
/>
<Action
ID="CreateStampedPose"
Expand All @@ -30,8 +34,8 @@
/>
<Action
ID="AddPoseStampedToVector"
input_pose="{stamped_pose}"
pose_stamped_vector="{pose_stamped_vector}"
input="{stamped_pose}"
vector="{pose_stamped_vector}"
/>
<Action
ID="CreateStampedPose"
Expand All @@ -42,8 +46,8 @@
/>
<Action
ID="AddPoseStampedToVector"
input_pose="{stamped_pose}"
pose_stamped_vector="{pose_stamped_vector}"
input="{stamped_pose}"
vector="{pose_stamped_vector}"
/>
<Action
ID="CreateStampedPose"
Expand All @@ -54,8 +58,8 @@
/>
<Action
ID="AddPoseStampedToVector"
input_pose="{stamped_pose}"
pose_stamped_vector="{pose_stamped_vector}"
input="{stamped_pose}"
vector="{pose_stamped_vector}"
/>
</Control>
<Decorator ID="KeepRunningUntilFailure">
Expand All @@ -65,13 +69,14 @@
path="{pose_stamped_vector}"
planning_group_name="manipulator"
tip_link="end_effector_link"
tip_offset="0.0;0.0;0.0"
tip_offset="0.0;0.0;0.0;0.0;0.0;0.0"
position_only="false"
blending_radius="0.02"
velocity_scale_factor="0.5"
acceleration_scale_factor="0.5"
trajectory_sampling_rate="100"
joint_trajectory_msg="{joint_trajectory_msg}"
debug_solution="{debug_solution}"
/>
<Action
ID="GetCurrentPlanningScene"
Expand All @@ -84,11 +89,14 @@
joint_trajectory_msg="{joint_trajectory_msg}"
joint_space_step="0.05"
cartesian_space_step="0.02"
debug_solution="{debug_solution}"
/>
<Action
ID="ExecuteFollowJointTrajectory"
execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
joint_trajectory_msg="{joint_trajectory_msg}"
goal_position_tolerance="0.000000"
goal_time_tolerance="0.000000"
/>
</Control>
</Decorator>
Expand Down
141 changes: 122 additions & 19 deletions src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/request_teleoperation.xml
View file
Original file line number Diff line number Diff line change
@@ -1,11 +1,12 @@
<?xml version="1.0" encoding="UTF-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Request Teleoperation">
<!-- The integer value used here for teleoperation mode comes from the moveit_studio_sdk_msgs/TeleoperationMode ROS message. -->
<!--The integer value used here for teleoperation mode comes from the moveit_studio_sdk_msgs/TeleoperationMode ROS message.-->
<BehaviorTree
ID="Request Teleoperation"
_description="Handles the different variations of teleoperation from the web UI, with the option of interactive user prompts and choosing the initial mode. Should be used as a subtree with port remapping as part of an another Objective."
_favorite="false"
_hardcoded="false"
_subtreeOnly="true"
>
<Control ID="Sequence">
<Action ID="Script" code="teleop_mode := 0" />
Expand All @@ -19,39 +20,141 @@
/>
<Decorator ID="KeepRunningUntilFailure">
<Control ID="Sequence">
<!-- Closing and opening the gripper -->
<!--Closing and opening the gripper-->
<Decorator ID="ForceSuccess" _skipIf="teleop_mode != 7">
<SubTree ID="Close Gripper" />
</Decorator>
<Decorator ID="ForceSuccess" _skipIf="teleop_mode != 6">
<SubTree ID="Open Gripper" />
</Decorator>
<!-- Joint sliders interpolate to joint state -->
<!--Joint sliders interpolate to joint state-->
<Decorator ID="ForceSuccess" _while="teleop_mode == 5">
<SubTree ID="Interpolate to Joint State" />
<Control ID="Sequence">
<Control ID="Fallback" name="root">
<Control ID="Sequence">
<Action
ID="RetrieveJointStateParameter"
timeout_sec="-1"
joint_state="{target_joint_state}"
/>
<SubTree
ID="Interpolate to Joint State"
_collapsed="false"
target_joint_state="{target_joint_state}"
/>
<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>
</Control>
</Decorator>
<!-- Interactive markers move to pose -->
<!--Interactive markers move to pose-->
<Decorator ID="ForceSuccess" _while="teleop_mode == 4">
<SubTree ID="Move to Pose" />
<Control ID="Sequence">
<Control ID="Fallback" name="root">
<Control ID="Sequence">
<Action
ID="RetrievePoseParameter"
timeout_sec="-1"
pose="{target_pose}"
/>
<SubTree
ID="Move to Pose"
_collapsed="false"
target_pose="{target_pose}"
/>
<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>
</Control>
</Decorator>
<!-- Waypoint buttons move to joint state -->
<!--Waypoint buttons move to joint state-->
<Decorator ID="ForceSuccess" _while="teleop_mode == 3">
<SubTree ID="Move to Joint State" />
<Control ID="Sequence">
<Control ID="Fallback" name="root">
<Control ID="Sequence">
<Action
ID="RetrieveJointStateParameter"
timeout_sec="-1"
joint_state="{target_joint_state}"
/>
<SubTree
ID="Move to Joint State"
_collapsed="false"
target_joint_state="{target_joint_state}"
/>
<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>
</Control>
</Decorator>
<!-- Cartesian and joint jogging -->
<Action
ID="TeleoperateTwist"
controller_name="servo_controller"
_while="teleop_mode == 2"
/>
<Action
ID="TeleoperateJointJog"
controller_name="servo_controller"
_while="teleop_mode == 1"
/>
<!--Cartesian and joint jogging-->
<Control ID="Sequence" _while="teleop_mode == 2">
<Action
ID="TeleoperateTwist"
controller_name="servo_controller"
/>
</Control>
<Control ID="Sequence" _while="teleop_mode == 1">
<Action
ID="TeleoperateJointJog"
controller_name="servo_controller"
/>
</Control>
</Control>
</Decorator>
</Control>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Request Teleoperation">
<input_port name="enable_user_interaction" default="false" />
<input_port name="user_interaction_prompt" default="" />
<input_port name="initial_teleop_mode" default="3" />
<MetadataFields>
<Metadata subcategory="User Input" />
</MetadataFields>
</SubTree>
</TreeNodesModel>
</root>
3 changes: 2 additions & 1 deletion ...pro_kinova_configs/kinova_gen3_mujoco_config/objectives/april_tag_object_registration.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,8 @@
<Control ID="Sequence">
<Action
ID="LoadPointCloudFromFile"
file_path="~/user_ws/install/picknik_accessories/share/picknik_accessories/descriptions/miscellaneous/mock_satellite.stl"
file_path="descriptions/miscellaneous/mock_satellite.stl"
package_name="picknik_accessories"
frame_id="object_frame"
num_sampled_points="3000"
random_seed="1234"
Expand Down
3 changes: 2 additions & 1 deletion src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,8 @@
<Control ID="Sequence">
<Action
ID="LoadPointCloudFromFile"
file_path="~/user_ws/install/picknik_accessories/share/picknik_accessories/descriptions/miscellaneous/mock_satellite.stl"
file_path="descriptions/miscellaneous/mock_satellite.stl"
package_name="picknik_accessories"
frame_id="object_frame"
num_sampled_points="3000"
random_seed="1234"
Expand Down
3 changes: 2 additions & 1 deletion ...eit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/compliant_grasp_rafti_vfc.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,8 @@
<Control ID="Sequence">
<Action
ID="LoadPointCloudFromFile"
file_path="~/user_ws/install/picknik_accessories/share/picknik_accessories/descriptions/miscellaneous/mock_satellite.stl"
file_path="descriptions/miscellaneous/mock_satellite.stl"
package_name="picknik_accessories"
frame_id="object_frame"
num_sampled_points="3000"
random_seed="1234"
Expand Down
27 changes: 25 additions & 2 deletions src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/move_along_path.xml
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
<?xml version="1.0" encoding="UTF-8" ?>
<?xml version="1.0" encoding="utf-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Move Along Path">
<!--//////////-->
<BehaviorTree
Expand All @@ -8,6 +8,19 @@
_hardcoded="false"
>
<Control ID="Sequence" name="TopLevelSequence">
<SubTree
ID="Move to Waypoint"
_collapsed="false"
acceleration_scale_factor="1.0"
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
controller_names="/joint_trajectory_controller"
joint_group_name="manipulator"
keep_orientation="false"
keep_orientation_tolerance="0.05"
link_padding="0.01"
velocity_scale_factor="1.0"
waypoint_name="View Satellite"
/>
<Control ID="Sequence">
<Action
ID="ActivateControllers"
Expand Down Expand Up @@ -69,14 +82,16 @@
path="{pose_stamped_vector}"
planning_group_name="manipulator"
tip_link="end_effector_link"
tip_offset="0.0;0.0;0.0"
tip_offset="0.0;0.0;0.0;0.0;0.0;0.0"
position_only="false"
blending_radius="0.02"
velocity_scale_factor="0.5"
acceleration_scale_factor="0.5"
trajectory_sampling_rate="100"
joint_trajectory_msg="{joint_trajectory_msg}"
debug_solution="{debug_solution}"
ik_cartesian_space_density="0.010000"
ik_joint_space_density="0.100000"
/>
<Action
ID="GetCurrentPlanningScene"
Expand All @@ -97,9 +112,17 @@
joint_trajectory_msg="{joint_trajectory_msg}"
goal_position_tolerance="0.000000"
goal_time_tolerance="0.000000"
goal_duration_tolerance="-1.000000"
/>
</Control>
</Decorator>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Move Along Path">
<MetadataFields>
<Metadata runnable="true" />
</MetadataFields>
</SubTree>
</TreeNodesModel>
</root>
Loading

0 comments on commit bcd5add

Please sign in to comment.