diff --git a/src/picknik_ur_base_config/config/config.yaml b/src/picknik_ur_base_config/config/config.yaml index 9a2890d1..a558ddfb 100644 --- a/src/picknik_ur_base_config/config/config.yaml +++ b/src/picknik_ur_base_config/config/config.yaml @@ -183,8 +183,6 @@ ros2_control: # [Optional, default=[]] controllers_inactive_at_startup: - "joint_trajectory_controller" - - "admittance_controller_open_door" - - "joint_trajectory_controller_chained_open_door" - "joint_trajectory_admittance_controller" - "velocity_force_controller" # Any controllers here will not be spawned by MoveIt Pro. diff --git a/src/picknik_ur_base_config/config/control/picknik_ur.ros2_control.yaml b/src/picknik_ur_base_config/config/control/picknik_ur.ros2_control.yaml index 138f849a..17851768 100644 --- a/src/picknik_ur_base_config/config/control/picknik_ur.ros2_control.yaml +++ b/src/picknik_ur_base_config/config/control/picknik_ur.ros2_control.yaml @@ -15,10 +15,6 @@ controller_manager: type: robotiq_controllers/RobotiqActivationController servo_controller: type: joint_trajectory_controller/JointTrajectoryController - admittance_controller_open_door: - type: admittance_controller/AdmittanceController - joint_trajectory_controller_chained_open_door: - type: joint_trajectory_controller/JointTrajectoryController joint_trajectory_admittance_controller: type: joint_trajectory_admittance_controller/JointTrajectoryAdmittanceController velocity_force_controller: @@ -81,47 +77,6 @@ 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 - servo_controller: ros__parameters: joints: @@ -172,97 +127,6 @@ 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 - joint_trajectory_admittance_controller: ros__parameters: joints: diff --git a/src/picknik_ur_base_config/objectives/3_waypoint_pick_and_place.xml b/src/picknik_ur_base_config/objectives/3_waypoint_pick_and_place.xml deleted file mode 100644 index 16438392..00000000 --- a/src/picknik_ur_base_config/objectives/3_waypoint_pick_and_place.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/picknik_ur_site_config/config/config.yaml b/src/picknik_ur_site_config/config/config.yaml index 7575353f..2af4bb53 100644 --- a/src/picknik_ur_site_config/config/config.yaml +++ b/src/picknik_ur_site_config/config/config.yaml @@ -13,3 +13,35 @@ objectives: custom_objectives: package_name: "picknik_ur_site_config" relative_path: "objectives" + +# Configuration for launching ros2_control processes. +# [Required, if using ros2_control] +ros2_control: + config: + package: "picknik_ur_site_config" + path: "config/control/picknik_ur.ros2_control.yaml" + # MoveIt Pro will load and activate these controllers at start up to ensure they are available. + # If not specified, it is up to the user to ensure the appropriate controllers are active and available + # for running the application. + # [Optional, default=[]] + controllers_active_at_startup: + - "force_torque_sensor_broadcaster" + - "robotiq_gripper_controller" + - "joint_state_broadcaster" + - "servo_controller" + - "io_and_status_controller" + - "robotiq_activation_controller" + # Load but do not start these controllers so they can be activated later if needed. + # [Optional, default=[]] + controllers_inactive_at_startup: + - "joint_trajectory_controller" + - "admittance_controller_open_door" + - "joint_trajectory_controller_chained_open_door" + - "joint_trajectory_admittance_controller" + - "velocity_force_controller" + # Any controllers here will not be spawned by MoveIt Pro. + # [Optional, default=[]] + controllers_not_managed: [] + # Optionally configure remapping rules to let multiple controllers receive commands on the same topic. + # [Optional, default=[]] + controller_shared_topics: [] diff --git a/src/picknik_ur_site_config/config/control/picknik_ur.ros2_control.yaml b/src/picknik_ur_site_config/config/control/picknik_ur.ros2_control.yaml new file mode 100644 index 00000000..138f849a --- /dev/null +++ b/src/picknik_ur_site_config/config/control/picknik_ur.ros2_control.yaml @@ -0,0 +1,330 @@ +controller_manager: + ros__parameters: + update_rate: 600 # Hz + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + io_and_status_controller: + type: ur_controllers/GPIOController + force_torque_sensor_broadcaster: + type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + robotiq_gripper_controller: + type: position_controllers/GripperActionController + robotiq_activation_controller: + type: robotiq_controllers/RobotiqActivationController + servo_controller: + type: joint_trajectory_controller/JointTrajectoryController + admittance_controller_open_door: + type: admittance_controller/AdmittanceController + joint_trajectory_controller_chained_open_door: + type: joint_trajectory_controller/JointTrajectoryController + joint_trajectory_admittance_controller: + type: joint_trajectory_admittance_controller/JointTrajectoryAdmittanceController + velocity_force_controller: + type: velocity_force_controller/VelocityForceController + +io_and_status_controller: + ros__parameters: + tf_prefix: "" + +force_torque_sensor_broadcaster: + ros__parameters: + sensor_name: tcp_fts_sensor + state_interface_names: + - force.x + - force.y + - force.z + - torque.x + - torque.y + - torque.z + frame_id: tool0 + topic_name: ft_data + +joint_trajectory_controller: + 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 + command_joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - 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.05 + shoulder_lift_joint: + goal: 0.05 + elbow_joint: + goal: 0.05 + wrist_1_joint: + goal: 0.05 + wrist_2_joint: + goal: 0.05 + 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 + +servo_controller: + 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 + command_joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - 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.05 + shoulder_lift_joint: + goal: 0.05 + elbow_joint: + goal: 0.05 + wrist_1_joint: + goal: 0.05 + wrist_2_joint: + goal: 0.05 + wrist_3_joint: + goal: 0.05 + +robotiq_gripper_controller: + ros__parameters: + default: true + joint: robotiq_85_left_knuckle_joint + allow_stalling: true + +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 + +joint_trajectory_admittance_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + base_frame: base_link + sensor_frame: tool0 + ee_frame: grasp_link + ft_sensor_name: tcp_fts_sensor + # Joint accelerations chosen to match MoveIt configs. + stop_accelerations: + - 30.0 + - 30.0 + - 30.0 + - 30.0 + - 30.0 + - 30.0 + +velocity_force_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + base_frame: base_link + sensor_frame: tool0 + ee_frame: grasp_link + ft_sensor_name: tcp_fts_sensor + ft_force_deadband: 2.0 + ft_torque_deadband: 1.0 + max_joint_velocity: + - 0.524 + - 0.524 + - 0.524 + - 1.047 + - 1.047 + - 1.047 + max_joint_acceleration: + - 52.4 + - 52.4 + - 52.4 + - 52.4 + - 52.4 + - 52.4 + max_cartesian_velocity: + - 0.25 + - 0.25 + - 0.25 + - 1.5707 + - 1.5707 + - 1.5707 + max_cartesian_acceleration: + - 20.0 + - 20.0 + - 20.0 + - 40.0 + - 40.0 + - 40.0