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!