How to use position_controllers/JointTrajectoryController when simulate UR in gazebo

[zouhbwll]

ROS: kinetic
Gazebo version: 9.12.0
universal_robot package version: 1.2.7

I want to simulate a UR5 with a robotiq 2f gripper(with mimicjoint) in Gazebo, and it tells me that No p gain specified for pid and I should provide pid parameters.

1. However, when p value of the UR5 is small(0-2), UR’s joints are shaking, and I can’t tune these joints with bar in rqt_joint_trajectory_controller.
   p=1
   
   p=2
   

2. when those parameters is bigger than 3, those joints seem to be fixed, I can’t tune these joints with bar in rqt_joint_trajectory_controller.

3. Especially for the elbow joint, the greater of the p-value, the stronger it will contractive. Once
   it is bigger than 2.5, it seems to have a self-collision.
   p=3
   
   p=10
   

4. If I only specify the pid parameters for the gripper, I also can not tune UR’s joints through bars in rqt_joint_trajectory_controller. And after a few senconds, the gripper will break up. By the way, the gripper can be simulated in gazebo without the arm.
   

===============================

• arm_control.yaml

arm_controller:
  type: position_controllers/JointTrajectoryController
  joints:
     - shoulder_pan_joint
     - shoulder_lift_joint
     - elbow_joint
     - wrist_1_joint
     - wrist_2_joint
     - wrist_3_joint
  constraints:
      goal_time: 0.6
      stopped_velocity_tolerance: 0.05
      shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
      shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
      elbow_joint: {trajectory: 0.1, goal: 0.1}
      wrist_1_joint: {trajectory: 0.1, goal: 0.1}
      wrist_2_joint: {trajectory: 0.1, goal: 0.1}
      wrist_3_joint: {trajectory: 0.1, goal: 0.1}
  stop_trajectory_duration: 0.5
  state_publish_rate:  25
  action_monitor_rate: 10

joint_group_position_controller:
  type: position_controllers/JointGroupPositionController
  joints:
     - shoulder_pan_joint
     - shoulder_lift_joint
     - elbow_joint
     - wrist_1_joint
     - wrist_2_joint
     - wrist_3_joint

===============================

• gripper_controller.yaml

gripper_controller:
  type: position_controllers/GripperActionController
  joint: finger_joint
  action_monitor_rate: 20
  goal_tolerance: 0.002
  max_effort: 100
  stall_velocity_threshold: 0.001
  stall_timeout: 1.0

===============================

• gazebo_control.yaml

gazebo_ros_control:
  pid_gains:
    shoulder_pan_joint:
      p: 2.0
      i: 0.05
      d: 0.0001
      i_clamp: 1

    shoulder_lift_joint:
      p: 2.0
      i: 0.05
      d: 0.0001
      i_clamp: 1

    ………………

    finger_joint:
      p: 20.0
      i: 0.1
      d: 0.0
      i_clamp: 0.2
      antiwindup: false
      publish_state: true
    
    left_inner_knuckle_joint:
      p: 20.0
      i: 0.1
      d: 0.0
      i_clamp: 0.2
      antiwindup: false
      publish_state: true

     ………………

===============================

• gazebo.launch

<?xml version="1.0"?>
<launch>
  <arg name="paused" default="false" doc="Starts gazebo in paused mode" />
  <arg name="gui" default="true" doc="Starts gazebo gui" />
  
  <!-- startup simulated world -->
  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" default="worlds/empty.world"/>
    <arg name="paused" value="$(arg paused)"/>
    <arg name="gui" value="$(arg gui)"/>
  </include>

  <!-- send robot urdf to param server -->
 <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find lab_ur5_robotiq2f140_description)/urdf/lab_ur5_robotiq2f140.urdf.xacro'" />

  <!-- Load gazebo controller configurations -->
  <!-- Note: You MUST load these PID parameters for all joints that are using
       the PositionJointInterface, otherwise the arm + gripper will act like a
       giant parachute, counteracting gravity, and causing some of the wheels
       to lose contact with the ground, so the robot won't be able to properly
       navigate. See
       https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612 -->
 <rosparam file="$(find lab_ur5_robotiq2f140_description)/cfg/gazebo_controller.yaml" command="load" />

 <!-- push robot_description to factory and spawn robot in gazebo -->
 <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" args="-z 2.0 -unpause -urdf -model robot -param robot_description" respawn="false" output="screen" />

 <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
   <param name="publish_frequency" type="double" value="30.0" />
 </node>

 <node name="rviz" pkg="rviz" type="rviz" args="-d $(find lab_ur5_robotiq2f140_description)/cfg/view_robot.rviz" required="true" />

  <!-- joint_state_controller -->
  <rosparam file="$(find  lab_ur5_robotiq2f140_description)/controller/joint_state_controller.yaml" command="load"/>
  <node name="joint_state_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn joint_state_controller" respawn="false" output="screen"/>
  <!-- gripper_controller -->
  <rosparam file="$(find lab_ur5_robotiq2f140_description)/controller/gripper_controller.yaml" command="load"/> 
  <node name="gripper_controller_spawner" pkg="controller_manager" type="spawner" args="gripper_controller --shutdown-timeout 0.5" />

  <!-- start this controller -->
  <rosparam file="$(find ur_gazebo)/controller/arm_controller_ur5.yaml" command="load"/>
  <node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn arm_controller" respawn="false" output="screen"/>

  <!-- load other controllers -->

  <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false" output="screen" args="load joint_group_position_controller" />
  <!-- Load teleop -->
  <node name="rqt_joint_trajectory_controller" pkg="rqt_joint_trajectory_controller" type="rqt_joint_trajectory_controller" />

</launch>

===============================
by the way , there is a ERROR i don't know how to treat.

[ERROR] [1590638537.717410563, 0.158000000]: Tried to advertise a service that is already advertised in this node [/gazebo_ros_control/pid_gains/shoulder_pan_joint/set_parameters]
[ERROR] [1590638537.743302537, 0.158000000]: Tried to advertise a service that is already advertised in this node [/gazebo_ros_control/pid_gains/shoulder_lift_joint/set_parameters]
[ERROR] [1590638537.768973579, 0.158000000]: Tried to advertise a service that is already advertised in this node [/gazebo_ros_control/pid_gains/elbow_joint/set_parameters]
[ERROR] [1590638537.794019667, 0.158000000]: Tried to advertise a service that is already advertised in this node [/gazebo_ros_control/pid_gains/wrist_1_joint/set_parameters]
[ERROR] [1590638537.820540471, 0.158000000]: Tried to advertise a service that is already advertised in this node [/gazebo_ros_control/pid_gains/wrist_2_joint/set_parameters]
[ERROR] [1590638537.848918447, 0.158000000]: Tried to advertise a service that is already advertised in this node [/gazebo_ros_control/pid_gains/wrist_3_joint/set_parameters]
[ERROR] [1590638537.872821151, 0.158000000]: Tried to advertise a service that is already advertised in this node [/gazebo_ros_control/pid_gains/finger_joint/set_parameters]
[ERROR] [1590638537.890079747, 0.158000000]: Tried to advertise a service that is already advertised in this node [/controller_manager/list_controllers]
[ERROR] [1590638537.890118760, 0.158000000]: Tried to advertise a service that is already advertised in this node [/controller_manager/list_controller_types]
[ERROR] [1590638537.890138809, 0.158000000]: Tried to advertise a service that is already advertised in this node [/controller_manager/load_controller]
[ERROR] [1590638537.890159609, 0.158000000]: Tried to advertise a service that is already advertised in this node [/controller_manager/unload_controller]
[ERROR] [1590638537.890179860, 0.158000000]: Tried to advertise a service that is already advertised in this node [/controller_manager/switch_controller]
[ERROR] [1590638537.890204698, 0.158000000]: Tried to advertise a service that is already advertised in this node [/controller_manager/reload_controller_libraries]

===============================
I'm pretty new in ROS and Gazebo.
I'd be very grateful for any help.

[gavanderhoorn]

Please see #443 for a related PR.

Gazebo version: 9.12.0

the Gazebo simulation files in this repository were created when Gazebo 7.x was still considered "new". I'm not surprised things don't work exactly out-of-the-box on 9.x

Unfortunately, I don't use any of these things myself, so I cannot help you.

If someone from the community can assist, please do so.

[gavanderhoorn]

Note: #518 changes many things in ur_gazebo.

That PR has mostly been tested with Gazebo 9, and I haven't run into any real issues there.

[gavanderhoorn]

I'm going to close this due to inactivity.

It's been quite a while, so most parts involved have changed.

#518 changes quite a few things as well.

If this is still a problem we can re-open.