diff --git a/docker-compose.yaml b/docker-compose.yaml index 8b31bd90..0a998c41 100644 --- 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 index 6c08b0d0..68ffaede 100644 --- 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,6 +191,8 @@ 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: [] @@ -189,6 +200,12 @@ ros2_control: # [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 index 4f0bbf87..1ca614fa 100644 --- 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 index 787b2f23..99b37a20 100644 --- 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 index 218995ce..e227a556 100644 --- 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 index a2e235ae..2ed09c27 100644 --- 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 @@ + + + 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 index 382feea5..1633938e 100644 --- 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 index 8ef3b047..10fc6c5d 100644 --- 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] diff --git a/src/picknik_ur_base_config/config/realsense_config_high_accuracy.json b/src/picknik_ur_base_config/config/realsense_config_high_accuracy.json new file mode 100644 index 00000000..2a3ce516 --- /dev/null +++ b/src/picknik_ur_base_config/config/realsense_config_high_accuracy.json @@ -0,0 +1,48 @@ +{ + "ignoreSAD": "0", + "param-censusenablereg-udiameter": "9", + "param-censusenablereg-vdiameter": "9", + "param-censususize": "9", + "param-censusvsize": "9", + "param-disableraucolor": "0", + "param-disablesadcolor": "0", + "param-disablesadnormalize": "0", + "param-disablesloleftcolor": "0", + "param-disableslorightcolor": "1", + "param-lambdaad": "751", + "param-lambdacensus": "6", + "param-leftrightthreshold": "10", + "param-maxscorethreshb": "2893", + "param-medianthreshold": "796", + "param-minscorethresha": "4", + "param-neighborthresh": "108", + "param-raumine": "6", + "param-rauminn": "3", + "param-rauminnssum": "7", + "param-raumins": "2", + "param-rauminw": "2", + "param-rauminwesum": "12", + "param-regioncolorthresholdb": "0.785714", + "param-regioncolorthresholdg": "0.565558", + "param-regioncolorthresholdr": "0.985323", + "param-regionshrinku": "3", + "param-regionshrinkv": "0", + "param-robbinsmonrodecrement": "25", + "param-robbinsmonroincrement": "2", + "param-rsmdiffthreshold": "1.65625", + "param-rsmrauslodiffthreshold": "0.71875", + "param-rsmremovethreshold": "0.809524", + "param-scanlineedgetaub": "13", + "param-scanlineedgetaug": "15", + "param-scanlineedgetaur": "30", + "param-scanlinep1": "155", + "param-scanlinep1onediscon": "160", + "param-scanlinep1twodiscon": "59", + "param-scanlinep2": "190", + "param-scanlinep2onediscon": "507", + "param-scanlinep2twodiscon": "493", + "param-secondpeakdelta": "647", + "param-texturecountthresh": "0", + "param-texturedifferencethresh": "1722", + "param-usersm": "1" +} diff --git a/src/picknik_ur_base_config/description/gripper_and_camera.xacro b/src/picknik_ur_base_config/description/gripper_and_camera.xacro new file mode 100644 index 00000000..521ba63f --- /dev/null +++ b/src/picknik_ur_base_config/description/gripper_and_camera.xacro @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/picknik_ur_base_config/description/picknik_ur5e.xacro b/src/picknik_ur_base_config/description/picknik_ur5e.xacro index c21f7ae3..d222cf23 100644 --- a/src/picknik_ur_base_config/description/picknik_ur5e.xacro +++ b/src/picknik_ur_base_config/description/picknik_ur5e.xacro @@ -1,9 +1,10 @@ - + + @@ -28,15 +29,14 @@ - - - + - - + + + @@ -45,9 +45,9 @@ prefix="" parent="world" joint_limits_parameters_file="$(arg joint_limits_parameters_file)" - kinematics_parameters_file="$(arg kinematics_parameters_file)/$(arg name)/default_kinematics.yaml" - physical_parameters_file="$(arg physical_parameters_file)/$(arg name)/physical_parameters.yaml" - visual_parameters_file="$(arg visual_parameters_file)/$(arg name)/visual_parameters.yaml" + kinematics_parameters_file="$(arg kinematics_parameters_file)" + physical_parameters_file="$(arg physical_parameters_file)" + visual_parameters_file="$(arg visual_parameters_file)" use_fake_hardware="$(arg use_fake_hardware)" initial_positions="${xacro.load_yaml(initial_positions_file)}" headless_mode="$(arg headless_mode)" @@ -63,6 +63,11 @@ + + + + + diff --git a/src/picknik_ur_base_config/description/pinch_link.xacro b/src/picknik_ur_base_config/description/pinch_link.xacro new file mode 100644 index 00000000..4715d196 --- /dev/null +++ b/src/picknik_ur_base_config/description/pinch_link.xacro @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/picknik_ur_base_config/launch/hardware.launch.py b/src/picknik_ur_base_config/launch/hardware.launch.py index f34ad7a0..b0b8da80 100644 --- a/src/picknik_ur_base_config/launch/hardware.launch.py +++ b/src/picknik_ur_base_config/launch/hardware.launch.py @@ -1,19 +1,19 @@ # Copyright 2023 PickNik Inc. -# +# # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: -# +# # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. -# +# # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. -# +# # * Neither the name of the PickNik Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. -# +# # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE @@ -44,7 +44,7 @@ def generate_launch_description(): nodes_to_launch = [] # Do not launch any nodes if there are no configured cameras. - if not cameras_config: + if cameras_config is None: print( "No camera configuration found. Not launching any camera transform nodes." ) @@ -59,7 +59,7 @@ def generate_launch_description(): package="moveit_studio_agent", executable="camera_transforms_node", name="camera_transforms_node", - output="screen", + output="both", parameters=frame_pair_params, ) nodes_to_launch.append(camera_transforms_node) diff --git a/src/picknik_ur_base_config/launch/robot_drivers_to_persist.launch.py b/src/picknik_ur_base_config/launch/robot_drivers_to_persist.launch.py index 881c900f..72aa95a2 100644 --- a/src/picknik_ur_base_config/launch/robot_drivers_to_persist.launch.py +++ b/src/picknik_ur_base_config/launch/robot_drivers_to_persist.launch.py @@ -1,19 +1,19 @@ # Copyright 2023 PickNik Inc. -# +# # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: -# +# # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. -# +# # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. -# +# # * Neither the name of the PickNik Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. -# +# # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE @@ -45,7 +45,7 @@ def generate_launch_description(): package="ur_robot_driver", executable="dashboard_client", name="dashboard_client", - output="screen", + output="both", emulate_tty=True, parameters=[{"robot_ip": hardware_config["ip"]}], ) @@ -54,7 +54,7 @@ def generate_launch_description(): package="moveit_studio_ur_pstop_manager", executable="protective_stop_manager_node", name="protective_stop_manager_node", - output="screen", + output="both", parameters=[ { "controllers_default_active": controller_config.get( diff --git a/src/picknik_ur_base_config/launch/robot_drivers_to_restart.launch.py b/src/picknik_ur_base_config/launch/robot_drivers_to_restart.launch.py new file mode 100644 index 00000000..8a21ea20 --- /dev/null +++ b/src/picknik_ur_base_config/launch/robot_drivers_to_restart.launch.py @@ -0,0 +1,34 @@ +# Copyright 2023 PickNik Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the PickNik Inc. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +from launch import LaunchDescription + + +def generate_launch_description(): + return LaunchDescription() diff --git a/src/picknik_ur_base_config/launch/sim/hardware_sim.launch.py b/src/picknik_ur_base_config/launch/sim/hardware_sim.launch.py index cff1cb37..16d07421 100644 --- a/src/picknik_ur_base_config/launch/sim/hardware_sim.launch.py +++ b/src/picknik_ur_base_config/launch/sim/hardware_sim.launch.py @@ -1,19 +1,19 @@ # Copyright 2023 PickNik Inc. -# +# # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: -# +# # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. -# +# # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. -# +# # * Neither the name of the PickNik Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. -# +# # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE @@ -61,7 +61,7 @@ def generate_launch_description(): package="moveit_studio_agent", executable="camera_transforms_node", name="camera_transforms_node", - output="screen", + output="both", parameters=frame_pair_params, ) nodes_to_launch.append(camera_transforms_node) diff --git a/src/picknik_ur_base_config/launch/sim/robot_drivers_to_persist_sim.launch.py b/src/picknik_ur_base_config/launch/sim/robot_drivers_to_persist_sim.launch.py index f13aadb1..c6596305 100644 --- a/src/picknik_ur_base_config/launch/sim/robot_drivers_to_persist_sim.launch.py +++ b/src/picknik_ur_base_config/launch/sim/robot_drivers_to_persist_sim.launch.py @@ -1,19 +1,19 @@ # Copyright 2023 PickNik Inc. -# +# # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: -# +# # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. -# +# # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. -# +# # * Neither the name of the PickNik Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. -# +# # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE @@ -37,7 +37,7 @@ def generate_launch_description(): package="moveit_studio_ur_pstop_manager", executable="mock_ur_dashboard_client_node", name="dashboard_client", - output="screen", + output="both", ) return LaunchDescription([mock_dashboard_client]) diff --git a/src/picknik_ur_base_config/launch/sim/robot_drivers_to_restart_sim.launch.py b/src/picknik_ur_base_config/launch/sim/robot_drivers_to_restart_sim.launch.py new file mode 100644 index 00000000..8a21ea20 --- /dev/null +++ b/src/picknik_ur_base_config/launch/sim/robot_drivers_to_restart_sim.launch.py @@ -0,0 +1,34 @@ +# Copyright 2023 PickNik Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the PickNik Inc. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +from launch import LaunchDescription + + +def generate_launch_description(): + return LaunchDescription() diff --git a/src/picknik_ur_base_config/objectives/close_cabinet_door.xml b/src/picknik_ur_base_config/objectives/close_cabinet_door.xml new file mode 100644 index 00000000..4fe22a32 --- /dev/null +++ b/src/picknik_ur_base_config/objectives/close_cabinet_door.xml @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/picknik_ur_base_config/objectives/close_gripper.xml b/src/picknik_ur_base_config/objectives/close_gripper.xml index 544bc81b..10ae314a 100644 --- a/src/picknik_ur_base_config/objectives/close_gripper.xml +++ b/src/picknik_ur_base_config/objectives/close_gripper.xml @@ -1,8 +1,8 @@ - + - + diff --git a/src/picknik_ur_base_config/objectives/find_cuboids.xml b/src/picknik_ur_base_config/objectives/find_cuboids.xml new file mode 100644 index 00000000..636451f5 --- /dev/null +++ b/src/picknik_ur_base_config/objectives/find_cuboids.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/src/picknik_ur_base_config/objectives/find_cuboids_config.yaml b/src/picknik_ur_base_config/objectives/find_cuboids_config.yaml new file mode 100644 index 00000000..1265d62e --- /dev/null +++ b/src/picknik_ur_base_config/objectives/find_cuboids_config.yaml @@ -0,0 +1,23 @@ +FindSingularCuboids: + base_frame: "world" + + # Distance threshold used when fitting a plane to the supporting surface. + # Increasing this value lets us fit to an uneven or noisy surface at the cost of preventing segmentation of short objects. + # Reducing this value allows detecting small objects on a very flat surface, but this will produce bad results for uneven or noisy surfaces. + plane_model_threshold: 0.01 + + # Distance threshold used when identifying clusters of points corresponding to distinct objects. + # Increasing this value makes the algorithm combine nearby discontinuous points into the same object, which lets the algorithm tolerate noisy data without introducing false positives at the cost of reduced accuracy. + # Reducing this value allows recognizing objects that are close to each other as distinct objects, but could increase the false positive rate. + cluster_distance_threshold: 0.01 + + # These define a rectangular region of interest relative to base_frame. + # The settings here define a 2m x 2m x 1m box just below the XY plane of the world frame. + # Origin of the region: + crop_box_origin_x: -1.0 + crop_box_origin_y: -1.0 + crop_box_origin_z: -0.03 + # Extent of the region: + crop_box_size_x: 2.0 + crop_box_size_y: 2.0 + crop_box_size_z: 1.0 diff --git a/src/picknik_ur_base_config/objectives/inspect_surface.xml b/src/picknik_ur_base_config/objectives/inspect_surface.xml new file mode 100644 index 00000000..09274184 --- /dev/null +++ b/src/picknik_ur_base_config/objectives/inspect_surface.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml b/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml index e63d3dc8..d25f4a13 100644 --- a/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml +++ b/src/picknik_ur_base_config/objectives/interpolate_to_joint_state.xml @@ -1,17 +1,17 @@ - + - + - + - + diff --git a/src/picknik_ur_base_config/objectives/interpolate_to_named_state.xml b/src/picknik_ur_base_config/objectives/interpolate_to_named_state.xml new file mode 100644 index 00000000..b117d47b --- /dev/null +++ b/src/picknik_ur_base_config/objectives/interpolate_to_named_state.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/src/picknik_ur_base_config/objectives/looping_pick_and_place_object.xml b/src/picknik_ur_base_config/objectives/looping_pick_and_place_object.xml new file mode 100644 index 00000000..c8845223 --- /dev/null +++ b/src/picknik_ur_base_config/objectives/looping_pick_and_place_object.xml @@ -0,0 +1,95 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/picknik_ur_base_config/objectives/move_to_joint_state.xml b/src/picknik_ur_base_config/objectives/move_to_joint_state.xml index 48d71543..105ab516 100644 --- a/src/picknik_ur_base_config/objectives/move_to_joint_state.xml +++ b/src/picknik_ur_base_config/objectives/move_to_joint_state.xml @@ -1,6 +1,6 @@ - + diff --git a/src/picknik_ur_base_config/objectives/move_to_known_pose.xml b/src/picknik_ur_base_config/objectives/move_to_known_pose.xml index 8e2f9eff..53161707 100644 --- a/src/picknik_ur_base_config/objectives/move_to_known_pose.xml +++ b/src/picknik_ur_base_config/objectives/move_to_known_pose.xml @@ -1,18 +1,18 @@ - - + + - + - + - + diff --git a/src/picknik_ur_base_config/objectives/move_to_named_pose.xml b/src/picknik_ur_base_config/objectives/move_to_named_pose.xml index 32266b1d..3d3ab86f 100644 --- a/src/picknik_ur_base_config/objectives/move_to_named_pose.xml +++ b/src/picknik_ur_base_config/objectives/move_to_named_pose.xml @@ -1,13 +1,13 @@ - + - + diff --git a/src/picknik_ur_base_config/objectives/move_to_pose.xml b/src/picknik_ur_base_config/objectives/move_to_pose.xml index c9aeda4d..a16a58c4 100644 --- a/src/picknik_ur_base_config/objectives/move_to_pose.xml +++ b/src/picknik_ur_base_config/objectives/move_to_pose.xml @@ -1,7 +1,7 @@ - + @@ -13,6 +13,6 @@ - + diff --git a/src/picknik_ur_base_config/objectives/open_door_admittance_controller_config.yaml b/src/picknik_ur_base_config/objectives/open_door_admittance_controller_config.yaml new file mode 100644 index 00000000..d65ae477 --- /dev/null +++ b/src/picknik_ur_base_config/objectives/open_door_admittance_controller_config.yaml @@ -0,0 +1,52 @@ +UpdateAdmittanceController: + controller_name: "admittance_controller_open_door" + end_effector_frame: "grasp_link" + control_frame: "grasp_link" + admittance: + selected_axes: + x: True + y: True + z: True + rx: True + ry: True + rz: True + mass_position: # kg + x: 10.0 + y: 10.0 + z: 10.0 + min: 0.001 + max: 100.0 + mass_rotation: # kg*m^2 + x: 5.0 + y: 5.0 + z: 5.0 + min: 0.001 + max: 100.0 + stiffness_position: # N/m + x: 500.0 + y: 500.0 + z: 500.0 + min: 0.0 + max: 1000.0 + stiffness_rotation: # N*m/rad + x: 100.0 + y: 100.0 + z: 100.0 + min: 0.0 + max: 1000.0 + damping_ratio_position: # unitless + x: 5.0 + y: 5.0 + z: 5.0 + min: 0.0 + max: 100.0 + damping_ratio_rotation: # unitless + x: 5.0 + y: 5.0 + z: 5.0 + min: 0.0 + max: 100.0 + joint_damping: # 1/s + value: 5.0 + min: 0.0 + max: 100.0 diff --git a/src/picknik_ur_base_config/objectives/open_door_affordance_config.yaml b/src/picknik_ur_base_config/objectives/open_door_affordance_config.yaml new file mode 100644 index 00000000..c34c8ee8 --- /dev/null +++ b/src/picknik_ur_base_config/objectives/open_door_affordance_config.yaml @@ -0,0 +1,16 @@ +SetupMTCOpenDoorAffordance: + arm_group_name: "manipulator" + end_effector_group_name: "gripper" + end_effector_name: "moveit_ee" + ik_frame_name: "manual_grasp_link" + end_effector_closed_pose_name: "close" + end_effector_open_pose_name: "open" + + # List of controllers to use during the stages that interact with the door. + # For this objective, we want to enable admittance control. + door_opening_controllers: "/joint_trajectory_controller_chained_open_door /admittance_controller_open_door /robotiq_gripper_controller" + + approach_distance: 0.05 + translation_distance: 0.0 + rotation_distance: 60.0 + use_circular_arc: true diff --git a/src/picknik_ur_base_config/objectives/open_gripper.xml b/src/picknik_ur_base_config/objectives/open_gripper.xml index 177424c2..a05b6179 100644 --- a/src/picknik_ur_base_config/objectives/open_gripper.xml +++ b/src/picknik_ur_base_config/objectives/open_gripper.xml @@ -2,8 +2,8 @@ - + - + diff --git a/src/picknik_ur_base_config/objectives/open_lever_handle_door.xml b/src/picknik_ur_base_config/objectives/open_lever_handle_door.xml new file mode 100644 index 00000000..3416ee5c --- /dev/null +++ b/src/picknik_ur_base_config/objectives/open_lever_handle_door.xml @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/picknik_ur_base_config/objectives/open_lever_handle_door_config.yaml b/src/picknik_ur_base_config/objectives/open_lever_handle_door_config.yaml new file mode 100644 index 00000000..be8514c0 --- /dev/null +++ b/src/picknik_ur_base_config/objectives/open_lever_handle_door_config.yaml @@ -0,0 +1,29 @@ +GetDoorHandle: + minimum_door_handle_height: 0.02 # meters + # The frame where the door handle pose is given. Model root frame, + # e.g. "world", is typically the right choice. + door_handle_pose_frame_name: world + +SetupMTCOpenLeverHandleDoor: + arm_group_name: "manipulator" + end_effector_group_name: "gripper" + ik_frame_name: "grasp_link" + + # List of controllers to use during the stages that interact with the door. + # We are currently not using admittance control for this objective since it's not needed. + door_opening_controllers: "/joint_trajectory_controller /robotiq_gripper_controller" + + # The hand should be placed 75% of the way down the handle length when + # manipulating the handle. Valid value range [0, 1.0]. + handle_grasp_offset_percent: 0.75 + # The distance above the handle the robot should approach with before + # manipulating it. + above_handle_approach_distance: 0.03 # meters + # The distance the hand should move past the end of the handle to release it. + release_handle_distance: 0.03 # meters + # How many degrees to rotate the lever handle (use positive value for + # counter-clockwise rotation and negative value for clockwise rotation). + handle_twist_angle: 65.0 # degrees + approach_distance: 0.15 # meters + push_open_distance: 0.05 # meters + use_circular_arc: true diff --git a/src/picknik_ur_base_config/objectives/pick_object.xml b/src/picknik_ur_base_config/objectives/pick_object.xml new file mode 100644 index 00000000..0e658bc6 --- /dev/null +++ b/src/picknik_ur_base_config/objectives/pick_object.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/picknik_ur_base_config/objectives/pick_object_config.yaml b/src/picknik_ur_base_config/objectives/pick_object_config.yaml new file mode 100644 index 00000000..26710654 --- /dev/null +++ b/src/picknik_ur_base_config/objectives/pick_object_config.yaml @@ -0,0 +1,11 @@ +SetupMTCPickObject: + # The lift vector points to the direction of the positive z-axis of the frame marked as the world frame. + world_frame_name: "world" + arm_group_name: "manipulator" + end_effector_group_name: "gripper" + end_effector_name: "moveit_ee" + hand_frame_name: "manual_grasp_link" + end_effector_closed_pose_name: "close" + + approach_distance: 0.1 + lift_distance: 0.1 diff --git a/src/picknik_ur_base_config/objectives/pick_place_object.xml b/src/picknik_ur_base_config/objectives/pick_place_object.xml new file mode 100644 index 00000000..630744d1 --- /dev/null +++ b/src/picknik_ur_base_config/objectives/pick_place_object.xml @@ -0,0 +1,112 @@ + + + + + +