Admittance Controller Demo

admittance_controller.repos

@@ -0,0 +1,51 @@
+repositories:
+
+  control_toolbox:
+    type: git
+    url: https://github.com/ros-controls/control_toolbox.git
+    verstion: demo-admittance-controller-delete-when-paramhandler-gravity-comp-merged
+
+  control_msgs: # Branch: rolling/add-addmittance-controller-state
+    type: git
+    url: https://github.com/nbbrooks/control_msgs.git
+    version: 81419078f8544e9e320f3405a4fa5d3a7ba3dc1e  # Add calculation values of the admittance rule
+
+  geometry2: # Branch: twist_wrench_transform_foxy
+    type: git
+    url: https://github.com/JafarAbdi/geometry2.git
+    version: 2888c23b97cb76674129314cc648b5b355bda3f4  # Add tomsg conversion for geometry_msgs/Transform
+
+  ros2_control:
+    type: git
+    url: https://github.com/destogl/ros2_control.git
+    version: admittance-controller-development
+
+  ros2_control_demos:
+    type: git
+    url: https://github.com/destogl/ros2_control_demos.git
+    version: admittance-controller-setup
+
+  ros2_controllers:
+    type: git
+    url: https://github.com/destogl/ros2_controllers.git
+    version: admittance-controller-demo
+
+  teleop_twist_keyboard: # Branch: wrench
+    type: git
+    url: https://github.com/nbbrooks/teleop_twist_keyboard.git
+    version: 63de1c17c4c5d0724018f3b5fd8139c472cd15ab  # Add Float64MultiArray publisher
+
+  Universal_Robots_Client_Library:  # Branch: master
+    type: git
+    url: https://github.com/UniversalRobots/Universal_Robots_Client_Library.git
+    version: 90338a76d65d86240710c8f976fd40296a49cd84  # Merge pull request #85 from jornb/feat/target_frequency
+
+  Universal_Robots_ROS2_Driver:  # Branch: main
+    type: git
+    url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git
+    version: 10f16b8869bb47aa82daf7bc05e0b62406ae8c5d  # Add ft_sensor frame.
+
+  ur_msgs:  # Branch: ros2
+    type: git
+    url: https://github.com/destogl/ur_msgs.git
+    version: d72e82719b72deef6a59ea50e5295817d0b4f9f6  # Corrected/updated dependencies

ros2_control_demo_bringup/config/admittance_demo_controllers.yaml

@@ -0,0 +1,277 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 600  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    io_and_status_controller:
+      type: ur_controllers/GPIOController
+
+    speed_scaling_state_broadcaster:
+      type: ur_controllers/SpeedScalingStateBroadcaster
+
+    force_torque_sensor_broadcaster:
+      type: ur_controllers/ForceTorqueStateBroadcaster
+
+    force_sensor_broadcaster:
+      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
+
+    joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    scaled_joint_trajectory_controller:
+      type: ur_controllers/ScaledJointTrajectoryController
+
+    forward_velocity_controller:
+      type: velocity_controllers/JointGroupVelocityController
+
+    admittance_controller:
+      type: admittance_controller/AdmittanceController
+
+    faked_forces_controller:
+      type: forward_command_controller/MultiInterfaceForwardCommandController
+
+
+speed_scaling_state_broadcaster:
+  ros__parameters:
+    state_publish_rate: 100.0
+
+
+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
+
+
+force_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: tcp_fts_sensor
+    frame_id: tool0
+    additional_frames_to_publish: [base_link]
+
+    sensor_filter_chain:
+      filter1:
+        type: control_filters/LowPassFilterWrench
+        name: low_pass_filter
+        params:
+          sampling_frequency: 200.0
+          damping_frequency: 50.0
+          damping_intensity: 1.0
+          divider: 1
+
+      filter2:
+        type: control_filters/GravityCompensationWrench
+        name: tool_gravity_compensation
+        params:
+          CoG:
+            x: 0.0
+            y: 0.0
+            z: 0.1
+          force: 0.0        # mass * 9.81
+          force_frame: wrist_3_link
+          sensor_frame: ft_frame
+          world_frame: base_link
+
+
+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
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    constraints:
+      stopped_velocity_tolerance: 0.2
+      goal_time: 0.0
+      shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
+      shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
+      elbow_joint: { trajectory: 0.2, goal: 0.1 }
+      wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
+      wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
+      wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
+
+
+scaled_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
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    constraints:
+      stopped_velocity_tolerance: 0.2
+      goal_time: 0.0
+      shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
+      shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
+      elbow_joint: { trajectory: 0.2, goal: 0.1 }
+      wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
+      wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
+      wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
+
+
+forward_velocity_controller:
+  ros__parameters:
+    joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    interface_name: velocity
+
+
+admittance_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
+
+    enable_parameter_update_without_reactivation: false
+
+    ft_sensor_name: tcp_fts_sensor
+    use_joint_commands_as_input: true
+    #joint_limiter_type: "joint_limits/SimpleJointLimiter"
+
+    state_publish_rate: 200.0 # Defaults to 50
+    action_monitor_rate: 20.0 # Defaults to 20
+
+    allow_partial_joints_goal: false # Defaults to false
+    open_loop_control: true
+    allow_integration_in_goal_trajectories: true
+    constraints:
+      stopped_velocity_tolerance: 0.01 # Defaults to 0.01
+      goal_time: 0.0 # Defaults to 0.0 (start immediately)
+
+    IK:
+      base: base_link # Assumed to be stationary
+      tip: tool0
+      group_name: ur5e_manipulator
+
+    endeffector_frame: tool0
+    control_frame: base_link # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector
+    fixed_world_frame: base_link  # Gravity points down (neg. Z) in this frame (Usually: world or base_link)
+    sensor_frame: ft_frame  # Wrench measurements are in this frame
+
+    admittance:
+      selected_axes:
+        x: true
+        y: true
+        z: true
+        rx: true
+        ry: true
+        rz: true
+
+      # Having ".0" at the end is MUST, otherwise there is a loading error
+      # F = M*a + D*v + S*(x - x_d)
+      mass:
+        x: 10.0
+        y: 10.0
+        z: 10.0
+        rx: 10.0
+        ry: 10.0
+        rz: 10.0
+
+      damping_ratio:    # damping can be used instead: zeta = D / (2 * sqrt( M * S ))
+        x: 2.828427
+        y: 2.828427
+        z: 2.828427
+        rx: 2.23607
+        ry: 2.23607
+        rz: 2.23607
+
+      stiffness:
+        x: 50.0
+        y: 50.0
+        z: 50.0
+        rx: 50.0
+        ry: 50.0
+        rz: 50.0
+
+    input_wrench_filter_chain:
+      filter1:
+        type: control_filters/LowPassFilterWrench
+        name: low_pass_filter
+        params:
+          sampling_frequency: 200.0
+          damping_frequency: 50.0
+          damping_intensity: 1.0
+          divider: 1
+
+      filter2:
+        type: control_filters/GravityCompensationWrench
+        name: wrist_gravity_compensation
+        params:
+          CoG:
+            x: 0.0
+            y: 0.0
+            z: 0.1
+          force: 0.0        # mass * 9.81
+          force_frame: wrist_3_link
+          sensor_frame: ft_frame
+          world_frame: base_link
+
+      filter3:
+        type: control_filters/GravityCompensationWrench
+        name: tool_gravity_compensation
+        params:
+          CoG:
+            x: 0.0
+            y: 0.0
+            z: 0.1
+          force: 0.0        # mass * 9.81
+          force_frame: wrist_3_link
+          sensor_frame: ft_frame
+          world_frame: base_link
+
+
+faked_forces_controller:
+  ros__parameters:
+    joint: tcp_fts_sensor
+    interface_names:
+      - force.x
+      - force.y
+      - force.z
+      - torque.x
+      - torque.y
+      - torque.z