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

Update kinova objectives #77

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Update kinova objectives #77

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4cd30f6
Update kinova objectives
dyackzan Jan 3, 2025
a48d1f0
added a movetowaypoint
MikeWrock Jan 8, 2025
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
28 changes: 18 additions & 10 deletions src/moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/move_along_path.xml
View file
Open in desktop
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I got this error when running this objective on the base config:
image
And this one in the mujoco sim:
image

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think it just needs a moveToWaypoint first so it's not in the zero configuration. The base config has a different initial position, but also a different objective

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
Open in desktop
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It looks like some of the joint limits are defined in degrees but interpreted as radians:
image

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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I added a movetowaypoint here and its working fine for me

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
Loading