Merge remote-tracking branch 'origin/main' into pr-teach_and_pick_fro…

…m_apriltag
PickNikRobotics · Oct 2, 2023 · cdd358e · cdd358e
2 parents 3ff50e1 + 9a56330
commit cdd358e
Show file tree

Hide file tree

Showing 15 changed files with 72 additions and 51 deletions.
diff --git a/src/picknik_ur_base_config/config/config.yaml b/src/picknik_ur_base_config/config/config.yaml
@@ -130,7 +130,7 @@ moveit_params:
     path: "config/moveit/stomp_planning.yaml"
   kinematics:
     package: "picknik_ur_base_config"
-    path: "config/moveit/pick_ik_kinematics.yaml"
+    path: "config/moveit/trac_ik_kinematics_distance.yaml"
   servo:
     package: "picknik_ur_base_config"
     path: "config/moveit/ur_servo.yaml"
@@ -139,7 +139,7 @@ moveit_params:
     path: "config/moveit/sensors_3d.yaml"
   servo_kinematics:
     package: "picknik_ur_base_config"
-    path: "config/moveit/kinematics.yaml"
+    path: "config/moveit/trac_ik_kinematics_speed.yaml"
   joint_limits:
     package: "picknik_ur_base_config"
     path: "config/moveit/joint_limits.yaml"

diff --git a/src/picknik_ur_base_config/config/moveit/trac_ik_kinematics_distance.yaml b/src/picknik_ur_base_config/config/moveit/trac_ik_kinematics_distance.yaml
@@ -0,0 +1,6 @@
+manipulator:
+  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
+  kinematics_solver_timeout: 0.005
+  kinematics_solver_attempts: 3
+  epsilon: 0.0001
+  solve_type: "Distance"
diff --git a/src/picknik_ur_base_config/config/moveit/trac_ik_kinematics_speed.yaml b/src/picknik_ur_base_config/config/moveit/trac_ik_kinematics_speed.yaml
@@ -0,0 +1,6 @@
+manipulator:
+  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
+  kinematics_solver_timeout: 0.005
+  kinematics_solver_attempts: 3
+  epsilon: 0.0001
+  solve_type: "Speed"
diff --git a/src/picknik_ur_base_config/config/moveit/ur_servo.yaml b/src/picknik_ur_base_config/config/moveit/ur_servo.yaml
@@ -19,7 +19,6 @@ override_velocity_scaling_factor: 0.5
 
 ## Properties of outgoing commands
 publish_period: 0.002  # 1/Nominal publish rate [seconds]
-low_latency_mode: false  # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
 
 # What type of topic does your robot driver expect?
 # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
@@ -31,9 +30,8 @@ publish_joint_velocities: false
 publish_joint_accelerations: false
 
 ## Plugins for smoothing outgoing commands
+use_smoothing: true
 smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
-
-## Incoming Joint State properties
 low_pass_filter_coeff: 1.5  # Larger --> trust the filtered data more, trust the measurements less.
 
 ## MoveIt properties
@@ -42,7 +40,7 @@ planning_frame: base_link  # The MoveIt planning frame. Often 'base_link' or 'wo
 is_primary_planning_scene_monitor: false  # The MoveGroup node maintains the planning scene, so Servo needs to get its world info from there.
 
 ## Other frames
-ee_frame_name: manual_grasp_link  # The name of the end effector link, used to return the EE pose
+ee_frame: manual_grasp_link  # The name of the end effector link, used to return the EE pose
 robot_link_command_frame: base_link  # commands must be given in the frame of a robot link. Usually either the base or end effector
 
 ## Stopping behaviour

diff --git a/src/picknik_ur_base_config/objectives/teleoperate.xml b/src/picknik_ur_base_config/objectives/teleoperate.xml
diff --git a/src/picknik_ur_base_config/objectives/teleoperate_joint_jog.xml b/src/picknik_ur_base_config/objectives/teleoperate_joint_jog.xml
@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+<root BTCPP_format="4" main_tree_to_execute="Teleoperate Joint Jog">
+    <BehaviorTree ID="Teleoperate Joint Jog" _description="Teleoperate the robot in joint jog mode from the Web UI">
+        <Control ID="Sequence" name="root">
+            <Action ID="TeleoperateJointJog" controller_name="streaming_controller"/>
+        </Control>
+    </BehaviorTree>
+</root>
diff --git a/src/picknik_ur_base_config/objectives/teleoperate_twist.xml b/src/picknik_ur_base_config/objectives/teleoperate_twist.xml
@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+<root BTCPP_format="4" main_tree_to_execute="Teleoperate Twist">
+    <BehaviorTree ID="Teleoperate Twist" _description="Teleoperate the robot in Cartesian twist mode from the Web UI">
+        <Control ID="Sequence" name="root">
+            <Action ID="TeleoperateTwist" controller_name="streaming_controller"/>
+        </Control>
+    </BehaviorTree>
+</root>
diff --git a/src/picknik_ur_base_config/package.xml b/src/picknik_ur_base_config/package.xml
@@ -22,6 +22,7 @@
   <exec_depend>picknik_accessories</exec_depend>
   <exec_depend>realsense2_description</exec_depend>
   <exec_depend>robotiq_description</exec_depend>
+  <exec_depend>trac_ik_kinematics_plugin</exec_depend>
   <exec_depend>ur_description</exec_depend>
   <exec_depend>ur_robot_driver</exec_depend>
 

diff --git a/src/picknik_ur_gazebo_config/config/config.yaml b/src/picknik_ur_gazebo_config/config/config.yaml
@@ -76,7 +76,7 @@ moveit_params:
     path: "config/moveit/sensors_3d.yaml"
   servo:
     package: "picknik_ur_gazebo_config"
-    path: "config/moveit/ur5e_servo.yaml"
+    path: "config/moveit/ur_servo.yaml"
 
 octomap_manager:
   # Input point cloud topic name. The *output* point cloud topic published by

diff --git a/...zebo_config/config/moveit/ur5e_servo.yaml → ...gazebo_config/config/moveit/ur_servo.yaml b/...zebo_config/config/moveit/ur5e_servo.yaml → ...gazebo_config/config/moveit/ur_servo.yaml
@@ -19,7 +19,6 @@ override_velocity_scaling_factor: 0.5
 
 ## Properties of outgoing commands
 publish_period: 0.005  # 1/Nominal publish rate [seconds]
-low_latency_mode: true  # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
 
 # What type of topic does your robot driver expect?
 # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
@@ -31,9 +30,8 @@ publish_joint_velocities: false
 publish_joint_accelerations: false
 
 ## Plugins for smoothing outgoing commands
+use_smoothing: true
 smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
-
-## Incoming Joint State properties
 low_pass_filter_coeff: 4.0  # Larger --> trust the filtered data more, trust the measurements less.
 
 ## MoveIt properties
@@ -42,11 +40,11 @@ planning_frame: base_link  # The MoveIt planning frame. Often 'base_link' or 'wo
 is_primary_planning_scene_monitor: false  # The MoveGroup node maintains the planning scene, so Servo needs to get its world info from there.
 
 ## Other frames
-ee_frame_name: manual_grasp_link  # The name of the end effector link, used to return the EE pose
+ee_frame: manual_grasp_link  # The name of the end effector link, used to return the EE pose
 robot_link_command_frame: base_link  # commands must be given in the frame of a robot link. Usually either the base or end effector
 
 ## Stopping behaviour
-incoming_command_timeout: 0.1  # Stop servoing if X seconds elapse without a new command
+incoming_command_timeout: 0.05  # Stop servoing if X seconds elapse without a new command
 
 ## Configure handling of singularities and joint limits
 lower_singularity_threshold: 30.0  # Start decelerating when the condition number hits this (close to singularity)

diff --git a/src/picknik_ur_mock_hw_config/config/moveit/ur_servo.yaml b/src/picknik_ur_mock_hw_config/config/moveit/ur_servo.yaml
@@ -38,7 +38,7 @@ planning_frame: base_link  # The MoveIt planning frame. Often 'base_link' or 'wo
 is_primary_planning_scene_monitor: false  # The MoveGroup node maintains the planning scene, so Servo needs to get its world info from there.
 
 ## Other frames
-ee_frame_name: manual_grasp_link  # The name of the end effector link, used to return the EE pose
+ee_frame: manual_grasp_link  # The name of the end effector link, used to return the EE pose
 robot_link_command_frame: base_link  # commands must be given in the frame of a robot link. Usually either the base or end effector
 
 ## Stopping behaviour

diff --git a/src/picknik_ur_mock_hw_config/objectives/constrained_pick_place.xml b/src/picknik_ur_mock_hw_config/objectives/constrained_pick_place.xml
@@ -2,14 +2,21 @@
 <root BTCPP_format="4" main_tree_to_execute="Constrained Pick and Place">
   <!-- ////////// -->
   <BehaviorTree ID="Constrained Pick and Place" _description="Pick and place an object with constraints on the gripper's range of motion" _favorite="true">
-    <Control ID="Sequence" name="TopLevelSequence">
-      <Action ID="MoveToWaypoint" waypoint_name="Grasp Left" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" constraints=""/>
-      <Action ID="InitializeMotionConstraints" constraints_name="gripper pointing down" constraints="{constraints}"/>
-      <Action ID="AppendOrientationConstraint" config_file_name="my_constraints.yaml" constraints="{constraints}"/>
-      <SubTree ID="Open Gripper"/>
-      <Action ID="MoveToWaypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="Gripper Down" use_all_planners="true" constraints="{constraints}"/>
-      <Action ID="MoveToWaypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="Grasp Right" use_all_planners="true" constraints="{constraints}"/>
-      <SubTree ID="Close Gripper"/>
-    </Control>
+    <Decorator ID="KeepRunningUntilFailure">
+      <Control ID="Sequence">
+        <Action ID="InitializeMotionConstraints" constraints_name="gripper pointing down" constraints="{constraints}"/>
+        <Action ID="AppendOrientationConstraint" config_file_name="my_constraints.yaml" constraints="{constraints}"/>
+        <Control ID="Sequence" name="TopLevelSequence">
+          <SubTree ID="Open Gripper"/>
+          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
+            <Action ID="MoveToWaypoint" waypoint_name="Grasp Right" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" constraints=""/>
+          </Decorator>
+          <SubTree ID="Close Gripper"/>
+          <Decorator ID="RetryUntilSuccessful" num_attempts="3">
+            <Action ID="MoveToWaypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planning_group_name="manipulator" waypoint_name="Grasp Left" use_all_planners="true" constraints="{constraints}"/>
+          </Decorator>
+        </Control>
+      </Control>
+    </Decorator>
   </BehaviorTree>
 </root>
diff --git a/src/picknik_ur_site_config/objectives/close_cabinet_door.xml b/src/picknik_ur_site_config/objectives/close_cabinet_door.xml
@@ -18,14 +18,13 @@
                 <Action ID="SplitMTCSolution" solution_in="{full_close_cabinet_door_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}" />
                 <Action ID="WaitForUserTrajectoryApproval" solution="{full_close_cabinet_door_solution}"/>
                 <Action ID="ExecuteMTCTask" solution="{move_to_approach_solution}"/>
-
                 <Action ID="SaveCurrentState" saved_robot_state="{pre_approach_robot_state}" />
-                <ForceSuccess>
-                    <ReactiveSequence>
-                        <Condition ID="IsForceWithinThreshold" parameters="{parameters}" />
-                        <Action ID="ExecuteMTCTask" solution="{push_solution}" />
-                    </ReactiveSequence>
-                </ForceSuccess>
+                <Control ID="Parallel" success_count="1" failure_count="1">
+                    <Decorator ID="Inverter">
+                        <Condition ID="IsForceWithinThreshold" parameters="{parameters}"/>
+                    </Decorator>
+                    <Action ID="ExecuteMTCTask" solution="{push_solution}"/>
+                </Control>
             </Control>
             <SubTree ID="RetreatToInitialPose" pre_approach_robot_state="{pre_approach_robot_state}" initial_robot_state="{initial_robot_state}" parameters="{parameters}" />
         </Control>

diff --git a/src/picknik_ur_site_config/objectives/push_button.xml b/src/picknik_ur_site_config/objectives/push_button.xml
@@ -18,14 +18,13 @@
                 <Action ID="SplitMTCSolution" solution_in="{full_push_button_solution}" index="3" solution_out_1="{move_to_approach_solution}" solution_out_2="{push_solution}" />
                 <Action ID="WaitForUserTrajectoryApproval" solution="{full_push_button_solution}"/>
                 <Action ID="ExecuteMTCTask" solution="{move_to_approach_solution}"/>
-
                 <Action ID="SaveCurrentState" saved_robot_state="{pre_approach_robot_state}" />
-                <ForceSuccess>
-                    <ReactiveSequence>
-                        <Condition ID="IsForceWithinThreshold" parameters="{parameters}" />
-                        <Action ID="ExecuteMTCTask" solution="{push_solution}" />
-                    </ReactiveSequence>
-                </ForceSuccess>
+                <Control ID="Parallel" success_count="1" failure_count="1">
+                    <Decorator ID="Inverter">
+                        <Condition ID="IsForceWithinThreshold" parameters="{parameters}"/>
+                    </Decorator>
+                    <Action ID="ExecuteMTCTask" solution="{push_solution}"/>
+                </Control>
             </Control>
             <SubTree ID="RetreatToInitialPose" pre_approach_robot_state="{pre_approach_robot_state}" initial_robot_state="{initial_robot_state}" parameters="{parameters}" />
         </Control>

diff --git a/src/picknik_ur_site_config/objectives/push_button_ml.xml b/src/picknik_ur_site_config/objectives/push_button_ml.xml
@@ -56,14 +56,13 @@
                 <Action ID="WaitForUserTrajectoryApproval"
                     solution="{full_push_along_axis_solution}" />
                 <Action ID="ExecuteMTCTask" solution="{move_to_approach_solution}" />
-
                 <Action ID="SaveCurrentState" saved_robot_state="{pre_approach_robot_state}" />
-                <ForceSuccess>
-                    <ReactiveSequence>
-                        <Condition ID="IsForceWithinThreshold" parameters="{parameters}" />
-                        <Action ID="ExecuteMTCTask" solution="{push_solution}" />
-                    </ReactiveSequence>
-                </ForceSuccess>
+                <Control ID="Parallel" success_count="1" failure_count="1">
+                    <Decorator ID="Inverter">
+                        <Condition ID="IsForceWithinThreshold" parameters="{parameters}"/>
+                    </Decorator>
+                    <Action ID="ExecuteMTCTask" solution="{push_solution}"/>
+                </Control>
             </Control>
             <SubTree ID="RetreatToInitialPose"
                 pre_approach_robot_state="{pre_approach_robot_state}"