Add Objectives and Configuration for PickNik UR Robots (#18)

* add objectives from base config * Update looping_pick_and_place_object.xml * remove parenthesis * Update objectives from main * Add additional foundational controllers * Update defaults and launch files * Update base moveit configuration for UR5e robots * Update base descriptions * Update GZ objectives * Add realsense high accuracy config file * Fix path typos for UR description * Adjust objective definition locations * Add mock hardware flag * Fix servo config * Additional objective parameterization * Update default compose file * Remove overriding push button objective * Remove tree nodes models * Add additional waypoints for all base objectives * Slow down default servo speed * Fix more copyrights * Add more waypoints for GZ config * Screen output for gz config * Update gz launch and site config * Remove sensor data qos * Fix paths * Spawn scene vs spawn cabinet * One more QoS * Update gz waypoints * Apply suggestions from code review Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> * Revert all non-critical changes to gz config * Update src/picknik_ur_base_config/objectives/looping_pick_and_place_object.xml Co-authored-by: Michael Wrock <mike.wrock@gmail.com> * External <-> Camera * Move Pick waypoint back * Adjust stall params for GAC in GZ sim * Fix whitespace * Remove unused config yaml * Remove unused servo param * Sequences -> control ids * Sequences -> control ids --------- Co-authored-by: Erik Holum <erik.holum@picknik.ai> Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Co-authored-by: Michael Wrock <mike.wrock@gmail.com> Co-authored-by: Sebastian Castro <sebas.a.castro@gmail.com>
PickNikRobotics · May 30, 2023 · c448100 · c448100
1 parent c43e0d5
commit c448100
Show file tree

Hide file tree

Showing 61 changed files with 1,833 additions and 933 deletions.
diff --git a/docker-compose.yaml b/docker-compose.yaml
@@ -24,7 +24,7 @@ services:
       # Provides a license key for the MoveIt Studio application to validate in the container.
       - STUDIO_LICENSE_KEY
       # Robot configuration package for deployment. Defaults to a mock hardware UR5e.
-      - STUDIO_CONFIG_PACKAGE=${STUDIO_CONFIG_PACKAGE:-picknik_001_ur5e_config}
+      - STUDIO_CONFIG_PACKAGE=${STUDIO_CONFIG_PACKAGE:-picknik_ur_site_config}
       # The directory on the host machine where MoveIt Studio configurations should be saved, such as new objectives.
       # Typically, this is "$HOME/.config/moveit_studio", which is the default.
       - STUDIO_HOST_CONFIG_DIR=${STUDIO_HOST_CONFIG_DIR:-$HOME/.config/moveit_studio}
@@ -77,6 +77,7 @@ services:
         condition: service_completed_successfully
     command: bash -c "source /build_user_ws.sh"
 
+  # Executes tests for STUDIO_HOST_USER_WORKSPACE. If STUDIO_HOST_USER_WORKSPACE is not defined, this service will do nothing.
   workspace_test:
     container_name: "moveit_studio_workspace_test"
     extends: base
@@ -129,8 +130,6 @@ services:
   # Starts the REST API for the Web UI
   rest_api:
     extends: base
-    environment:
-      - LOG_LEVEL=DEBUG
     healthcheck:
       test: "curl -f http://localhost:5001/objectives"
       interval: 5s

diff --git a/src/picknik_ur_base_config/config/base_config.yaml b/src/picknik_ur_base_config/config/base_config.yaml
@@ -14,8 +14,10 @@ hardware:
   type: ${UR_HARDWARE_TYPE:-ur5e}
 
   # Set simulated to false if you are using this as a configuration for real hardware.
+  # Override-able by setting the $MOCK_HARDWARE environment variable to 'True'.
+  # This allows users to switch between mock and real hardware using the same configuration.
   # [Required]
-  simulated: True
+  simulated: ${MOCK_HARDWARE:-true}
 
   # If the MoveIt Studio Agent should launch the ros2 controller node.
   # [Optional, default=True]
@@ -33,13 +35,16 @@ hardware:
 
   # The robot's IP address.
   # [Required]
-  ip: "0.0.0.0"
+  ip: "!!! Please set the IP address of the robot"
 
   # Specify additional launch files for running the robot with real hardware.
   # [Required]
   robot_driver_persist_launch_file:
     package: "picknik_ur_base_config"
     path: "launch/robot_drivers_to_persist.launch.py"
+  robot_driver_restart_launch_file:
+    package: "picknik_ur_base_config"
+    path: "launch/robot_drivers_to_restart.launch.py"
   hardware_launch_file:
     package: "picknik_ur_base_config"
     path: "launch/hardware.launch.py"
@@ -50,6 +55,9 @@ hardware:
   simulated_robot_driver_persist_launch_file:
     package: "picknik_ur_base_config"
     path: "launch/sim/robot_drivers_to_persist_sim.launch.py"
+  simulated_robot_driver_restart_launch_file:
+    package: "picknik_ur_base_config"
+    path: "launch/sim/robot_drivers_to_restart_sim.launch.py"
   simulated_hardware_launch_file:
     package: "picknik_ur_base_config"
     path: "launch/sim/hardware_sim.launch.py"
@@ -77,6 +85,7 @@ hardware:
       - name: "%>> hardware.type"
       - prefix: ""
       - use_fake_hardware: "%>> hardware.simulated"
+      - use_pinch_link: "true"
       - fake_sensor_commands: "false"
       - headless_mode: "true"
       - robot_ip: "%>> hardware.ip"
@@ -89,15 +98,15 @@ hardware:
       - kinematics_parameters_file:
       # Load default_kinematics.yaml from ur_description/config/ur5e
           package: "ur_description"
-          path: "config"
+          path: "config/ur5e/default_kinematics.yaml"
       - physical_parameters_file:
       # Load physical_parameters.yaml from ur_description/config/ur5e
           package: "ur_description"
-          path: "config"
+          path: "config/ur5e/physical_parameters.yaml"
       - visual_parameters_file:
       # Load visual_parameters.yaml from ur_description/config/ur5e
           package: "ur_description"
-          path: "config"
+          path: "config/ur5e/visual_parameters.yaml"
 
 # Sets ROS global params for launch.
 # [Optional]
@@ -182,13 +191,21 @@ ros2_control:
   # [Optional, default=[]]
   controllers_inactive_at_startup:
     - "joint_trajectory_controller"
+    - "admittance_controller_open_door"
+    - "joint_trajectory_controller_chained_open_door"
   # Any controllers here will not be spawned by MoveIt Studio.
   # [Optional, default=[]]
   controllers_not_managed: []
   # Optionally configure remapping rules to let multiple controllers receive commands on the same topic.
   # [Optional, default=[]]
   controller_shared_topics: []
 
+# Octomap manager configuration parameters
+octomap_manager:
+  # Input point cloud topic name. The *output* point cloud topic published by
+  # the Octomap manager node is defined in sensors_3d.yaml.
+  input_point_cloud_topic: "/wrist_mounted_camera/depth/color/points"
+
 # Configuration for loading behaviors and objectives. 
 # [Required]
 objectives:

diff --git a/src/picknik_ur_base_config/config/control/picknik_ur5e.ros2_control.yaml b/src/picknik_ur_base_config/config/control/picknik_ur5e.ros2_control.yaml
@@ -73,6 +73,47 @@ joint_trajectory_controller:
       wrist_3_joint:
         goal: 0.05
 
+joint_trajectory_controller_chained_open_door:
+  ros__parameters:
+    joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    command_interfaces:
+      - position
+      - velocity
+    state_interfaces:
+      - position
+      - velocity
+    command_joints:
+      - admittance_controller_open_door/shoulder_pan_joint
+      - admittance_controller_open_door/shoulder_lift_joint
+      - admittance_controller_open_door/elbow_joint
+      - admittance_controller_open_door/wrist_1_joint
+      - admittance_controller_open_door/wrist_2_joint
+      - admittance_controller_open_door/wrist_3_joint
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    constraints:
+      stopped_velocity_tolerance: 0.0
+      goal_time: 0.0
+      shoulder_pan_joint:
+        goal: 0.25
+      shoulder_lift_joint:
+        goal: 0.25
+      elbow_joint:
+        goal: 0.25
+      wrist_1_joint:
+        goal: 0.25
+      wrist_2_joint:
+        goal: 0.25
+      wrist_3_joint:
+        goal: 0.25
+
 streaming_controller:
   ros__parameters:
     joints:
@@ -96,3 +137,94 @@ robotiq_gripper_controller:
 robotiq_activation_controller:
   ros__parameters:
     default: true
+
+admittance_controller_open_door:
+  ros__parameters:
+    joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+
+    command_interfaces:
+      - position
+
+    state_interfaces:
+      - position
+      - velocity
+
+    chainable_command_interfaces:
+      - position
+      - velocity
+
+    kinematics:
+      plugin_name: kinematics_interface_kdl/KinematicsInterfaceKDL
+      plugin_package: kinematics_interface
+      base: base_link  # Assumed to be stationary
+      tip: grasp_link  # The end effector frame
+      alpha: 0.05
+
+    ft_sensor:
+      name: tcp_fts_sensor
+      frame:
+        id: wrist_3_link  # Wrench measurements are in this frame
+      filter_coefficient: 0.1
+
+    control:
+      frame:
+        id: grasp_link  # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector
+
+    fixed_world_frame:
+      frame:            # Gravity points down (neg. Z) in this frame (Usually: world or base_link)
+        id: base_link   # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector
+
+    gravity_compensation:
+      frame:
+        id: wrist_3_link
+
+      CoG:  # specifies the center of gravity of the end effector
+        pos:
+          - 0.1  # x
+          - 0.0  # y
+          - 0.0  # z
+        force: 0.0  # mass * 9.81
+
+    admittance:
+      selected_axes:
+        - true  # x
+        - true  # y
+        - true  # z
+        - true  # rx
+        - true  # ry
+        - true  # rz
+
+      # Having ".0" at the end is MUST, otherwise there is a loading error
+      # F = M*a + D*v + S*(x - x_d)
+      mass:
+        - 10.0  # x
+        - 10.0  # y
+        - 10.0  # z
+        - 5.0  # rx
+        - 5.0  # ry
+        - 5.0  # rz
+
+      damping_ratio:  # damping can be used instead: zeta = D / (2 * sqrt( M * S ))
+        - 5.0  # x
+        - 5.0  # y
+        - 5.0  # z
+        - 5.0  # rx
+        - 5.0  # ry
+        - 5.0  # rz
+
+      stiffness:
+        - 500.0  # x
+        - 500.0  # y
+        - 500.0  # z
+        - 100.0  # rx
+        - 100.0  # ry
+        - 100.0  # rz
+
+    # general settings
+    enable_parameter_update_without_reactivation: true
diff --git a/src/picknik_ur_base_config/config/moveit/joint_limits.yaml b/src/picknik_ur_base_config/config/moveit/joint_limits.yaml
@@ -80,3 +80,8 @@ joint_limits:
     max_position: !degrees  180.0
     max_velocity: !degrees  60.0
     min_position: !degrees -180.0
+  robotiq_85_left_knuckle_joint:
+    has_velocity_limits: true
+    max_velocity: 0.5
+    has_acceleration_limits: true
+    max_acceleration: 1.0
diff --git a/src/picknik_ur_base_config/config/moveit/pick_ik_kinematics_servo.yaml b/src/picknik_ur_base_config/config/moveit/pick_ik_kinematics_servo.yaml
@@ -10,6 +10,7 @@ manipulator:
   orientation_threshold: 0.001
   approximate_solution_position_threshold: 0.01
   approximate_solution_orientation_threshold: 0.01
+  approximate_solution_joint_threshold: 0.1
   cost_threshold: 0.001
   minimal_displacement_weight: 0.001
   avoid_joint_limits_weight: 0.0

diff --git a/src/picknik_ur_base_config/config/moveit/picknik_ur5e.srdf b/src/picknik_ur_base_config/config/moveit/picknik_ur5e.srdf
@@ -56,6 +56,9 @@
     <disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent"/>
     <disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent"/>
     <disable_collisions link1="forearm_link" link2="wrist_2_link" reason="Never"/>
+    <disable_collisions link1="pinch_link" link2="forearm_link" reason="Adjacent"/>
+    <disable_collisions link1="pinch_link" link2="upper_arm_link" reason="Never"/>
+    <disable_collisions link1="pinch_link" link2="wrist_1_link" reason="Never"/>
     <disable_collisions link1="realsense_camera_adapter_link" link2="robotiq_85_base_link" reason="Never"/>
     <disable_collisions link1="realsense_camera_adapter_link" link2="robotiq_85_left_finger_link" reason="Never"/>
     <disable_collisions link1="realsense_camera_adapter_link" link2="robotiq_85_left_finger_tip_link" reason="Never"/>

diff --git a/src/picknik_ur_base_config/config/moveit/pilz_cartesian_limits.yaml b/src/picknik_ur_base_config/config/moveit/pilz_cartesian_limits.yaml
@@ -1,6 +1,6 @@
 # Cartesian limits for the Pilz planner
 cartesian_limits:
-  max_trans_vel: 0.2
-  max_trans_acc: 0.25
-  max_trans_dec: -0.25
-  max_rot_vel: 0.2
+  max_trans_vel: 0.1
+  max_trans_acc: 0.1
+  max_trans_dec: -0.1
+  max_rot_vel: 0.1
diff --git a/src/picknik_ur_base_config/config/moveit/ur5e_servo.yaml b/src/picknik_ur_base_config/config/moveit/ur5e_servo.yaml
@@ -1,22 +1,21 @@
 ###############################################
 # Modify all parameters related to servoing here
 ###############################################
-use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
 
 ## Properties of incoming commands
 command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
 scale:
   # Scale parameters are only used if command_in_type=="unitless"
-  linear: 0.5  # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
-  rotational: 1.0 # Max angular velocity. Radians per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
+  linear: 2.0  # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
+  rotational: 3.0 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
   # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
-  joint: 0.5
+  joint: 2.0
 
-# Drive the arm at a fraction of the maximum speed between 0 and 1, where 1 is max.
-override_velocity_scaling_factor: 0.4
+# Default to driving the arm at 50% maximum speed.
+override_velocity_scaling_factor: 0.5
 
 ## Properties of outgoing commands
-publish_period: 0.01  # 1/Nominal publish rate [seconds]
+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?
@@ -31,6 +30,9 @@ publish_joint_accelerations: false
 ## Plugins for smoothing outgoing commands
 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
 move_group_name: manipulator  # Often 'manipulator' or 'arm'
 planning_frame: base_link  # The MoveIt planning frame. Often 'base_link' or 'world'
@@ -42,9 +44,6 @@ robot_link_command_frame: base_link  # commands must be given in the frame of a
 
 ## Stopping behaviour
 incoming_command_timeout: 0.1  # Stop servoing if X seconds elapse without a new command
-# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
-# Important because ROS may drop some messages and we need the robot to halt reliably.
-num_outgoing_halt_msgs_to_publish: 4
 
 ## Configure handling of singularities and joint limits
 lower_singularity_threshold: 30.0  # Start decelerating when the condition number hits this (close to singularity)
@@ -60,6 +59,6 @@ command_out_topic: /streaming_controller/commands # Publish outgoing commands he
 
 ## Collision checking for the entire robot body
 check_collisions: true # Check collisions?
-collision_check_rate: 50.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
+collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
 self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
-scene_collision_proximity_threshold: 0.05 # Start decelerating when a scene collision is this far [m]
+scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]