How to connect the controller with the gazebo

[yuweiDu]

Problem description Previously, I only used Rviz to do the simulation and the controller works really well. But I want to implement the force controller and for the safety reason, I want to implement it in the simulation. I want to add some objects as well as the ft sensor so I need gazebo. My problem is when I launch the robot it vibrates a lot. I don't know the reason.

https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/assets/28621665/40f77c75-9b58-4203-b179-08ba88e75194

Software versions

• Commit: What's the output of git rev-parse HEAD in the cartesian_controllers repository?
• OS: Ubuntu 20.04
• ROS version: ROS Noetic

To Reproduce If possible, list the steps to reproduce the behavior. To reproduce the problem, the launch file is as following:

<launch>
  <!-- Load hardware configuration -->
  <rosparam file="$(find ros_control_boilerplate)/frrobot_control/config/fr10_hw_config.yaml" command="load"></rosparam>
  <!-- Load controller configuration -->
  <rosparam file="$(find ros_control_boilerplate)/frrobot_control/config/fr10_controllers.yaml" command="load"></rosparam>

  <param name="robot_description" textfile="$(find frcobot_description)/urdf/fr10_robot_gazebo.urdf"/>
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="paused" value="true"/>
  </include>

  <node name="spawn" pkg="gazebo_ros" type="spawn_model" args="-param /robot_description -urdf -x 0 -y 0 -z 0.1 -model fr10">
    <remap from="my_cartesian_motion_controller/target_frame" to="target_frame" />
  </node>

  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" args="
                        my_motion_control_handle
                        my_cartesian_motion_controller 
                        joint_state_controller" >
  <remap from="my_cartesian_motion_controller/target_frame" to="target_frame" />
  </node>

  <!-- convert joint states to TF transforms for rviz, etc -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
    respawn="false" output="screen">
  </node>

</launch>

the fr10_controller.yaml file is as following:

my_cartesian_motion_controller:
    type: "position_controllers/CartesianMotionController"
    end_effector_link: "EE_link" #"tool0"
    robot_base_link: "base_link"
    target_frame_topic: "target_frame"
    joints:
    - j1
    - j2
    - j3
    - j4
    - j5
    - j6

    pd_gains:
        trans_x: {p: 10.0}
        trans_y: {p: 10.0}
        trans_z: {p: 10.0}
        rot_x: {p: 1.0}
        rot_y: {p: 1.0}
        rot_z: {p: 1.0}

my_cartesian_force_controller:
    type: "position_controllers/CartesianForceController"
    end_effector_link: "wrist3_Link" #"tool0"
    robot_base_link: "base_link"
    ft_sensor_ref_link: "sensor_link"
    joints:
    - j1
    - j2
    - j3
    - j4
    - j5
    - j6

    solver:
        error_scale: 0.5

    pd_gains:
        trans_x: {p: 0.05}
        trans_y: {p: 0.05}
        trans_z: {p: 0.05}
        rot_x: {p: 1.5}
        rot_y: {p: 1.5}
        rot_z: {p: 1.5}

my_cartesian_hybrid_motion_force_controller:
    type: "position_controllers/CartesianHybridMotionForceController"
    end_effector_link: "EE_link" #"tool0"
    robot_base_link: "base_link"
    ft_sensor_ref_link: "ft_sensor_link"
    joints:
    - j1
    - j2
    - j3
    - j4
    - j5
    - j6

    solver:
        error_scale: 0.5

    pd_gains:
        trans_x: {p: 10}
        trans_y: {p: 10}
        trans_z: {p: 0.05}
        rot_x: {p: 1.5}
        rot_y: {p: 1.5}
        rot_z: {p: 1.5}

my_cartesian_compliance_controller:
    type: "position_controllers/CartesianComplianceController"
    end_effector_link: "wrist3_Link" #"tool0"
    robot_base_link: "base_link"
    ft_sensor_ref_link: "sensor_link"
    compliance_ref_link: "wrist3_Link" #"tool0"
    target_frame_topic: "target_frame"
    joints:
    - j1
    - j2
    - j3
    - j4
    - j5
    - j6

    stiffness:  # w.r.t. compliance_ref_link
        trans_x: 500
        trans_y: 500
        trans_z: 500
        rot_x: 20
        rot_y: 20
        rot_z: 20

    solver:
        error_scale: 0.5

    pd_gains:
        trans_x: {p: 0.05}
        trans_y: {p: 0.05}
        trans_z: {p: 0.05}
        rot_x: {p: 1.50}
        rot_y: {p: 1.50}
        rot_z: {p: 1.50}

my_motion_control_handle:
   type: "cartesian_controllers/MotionControlHandle"
   end_effector_link: "wrist3_Link" #"tool0"
   robot_base_link: "base_link"
   target_frame_topic: "/my_cartesian_motion_controller/target_frame"
   joints:
    - j1
    - j2
    - j3
    - j4
    - j5
    - j6

my_joint_to_cartesian_controller:
    type: "cartesian_controllers/JointControllerAdapter"
    end_effector_link: "EE_link" #"tool0"
    robot_base_link: "base_link"
    target_frame_topic: "/my_cartesian_motion_controller/target_frame"
    joints:
    - j1
    - j2
    - j3
    - j4
    - j5
    - j6

    joint_limits:
      j1:
        has_velocity_limits: true
        max_velocity: 3.15
        has_acceleration_limits: true
        max_acceleration: 10.0
      j2:
        has_velocity_limits: true
        max_velocity: 3.15
        has_acceleration_limits: true
        max_acceleration: 10.0
      j3:
        has_velocity_limits: true
        max_velocity: 3.15
        has_acceleration_limits: true
        max_acceleration: 10.0
      j4:
        has_velocity_limits: true
        max_velocity: 3.2
        has_acceleration_limits: true
        max_acceleration: 10.0
      j5:
        has_velocity_limits: true
        max_velocity: 3.2
        has_acceleration_limits: true
        max_acceleration: 10.0
      j6:
        has_velocity_limits: true
        max_velocity: 3.2
        has_acceleration_limits: true
        max_acceleration: 10.0

my_joint_to_cartesian_controller/joint_trajectory_controller:
    type: "position_controllers/JointTrajectoryController"
    joints:
    - j1
    - j2
    - j3
    - j4
    - j5
    - j6

joint_trajectory_controller:
    type: "position_controllers/JointTrajectoryController"
    joints:
    - j1
    - j2
    - j3
    - j4
    - j5
    - j6

joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50

I am not sure if it's the urdf problem. But when I try to control the robot using the joint controller. It won't vibrate and can be controlled.But the problem is when starting the gazebo simulation the robot will jump at the first.

https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/assets/28621665/de95ccb6-5540-49f7-9e7e-e1560bb4cc28 When I publish command to the controller(rostopic pub /j1_controller/command std_msgs/Float64 "data: 2.0"), the robot moves

https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/assets/28621665/f99bab61-25ad-456c-9cf0-e5d301411cad

The launch file is :

```

The joint_controller.yaml is

j1_controller: type: position_controllers/JointPositionController joint: j1

j2_controller: type: position_controllers/JointPositionController joint: j2

j3_controller: type: position_controllers/JointPositionController joint: j3

j4_controller: type: position_controllers/JointPositionController joint: j4

j5_controller: type: position_controllers/JointPositionController joint: j5

j6_controller: type: position_controllers/JointPositionController joint: j6

joint_state_controller: type: "joint_state_controller/JointStateController" publish_rate: 50

Can anyone help? Thanks a lot!

[stefanscherzinger]

Hi @yuweiDu

Thanks for reporting this and sorry for not responding earlier. The first video really shows instable behavior. Could you activate the my_cartesian_motion_controller and set the error_scale to 0.0? The controller shouldn't command anything other than the current joint positions in that case. I would like to find out, if this is a general instability problem with Gazebo's collision solver or if indeed something is not working with the cartesian controller's algorithms.

Additionally, could you navigate into the cartesian_controllers package that you clone from github and run this command:

git rev-parse HEAD

It should give you a hash that tells us on which version you are working. Thanks.