Vibration in joints when I add gazebo_ros_control plugin [closed]

edit

I am using the xacro file given below to spawn a kuka robot in gazebo. Whenever I spawn the robot with this xacro file, I see small vibrations in some of the joints. However, if I comment out the < gazebo > tag that defines the control plugin (line 6 to 11 excluding empty lines), this vibration disappears.

Though this vibration is small, it creates a problem while using moveit as when I define an end pose and try to execute a planned path, I get the following error:

Invalid Trajectory: start point deviates from current robot state more than 0.01 joint 'kuka_arm_6_joint': expected: -0.683276, current: 2.20861

UPDATE: It seems that the problem is with joints 5, 6 and 7. If I remove these joints from the xacro the vibration disappears. Moreover, for joints 5 and 7, the controller is not able to move it to the set point correctly. No matter what set point is given, both joints stabilize at some other random point.

I was not able to add the videos to this question. So I have uploaded them to a google drive. Here's the link: https://drive.google.com/drive/folders/1Rtpe_22l3jOnsukvlULb_VTK5IvRVLMJ?usp=sharing

I have also pasted my launch files below the xacro file. (I cannot attach them to the question because I do not have enough points).

The xacro file is as follows:

<?xml version="1.0"?>

 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="kuka_lwr">

 <xacro:property name="M_PI" value="3.1415926535897931" />
 <xacro:property name="name" value="kuka" />
 <xacro:property name="inertia_scaling" value="1" />
  <gazebo>
      <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
        <robotNamespace>/kuka</robotNamespace>
        <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
      </plugin>
  </gazebo>

  <xacro:macro name="cuboid_inertia_def" params="width height length mass">
    <inertia ixx="${mass * (height * height + length * length) / 12 * inertia_scaling}"
             iyy="${mass * (width * width + height * height) / 12 * inertia_scaling}"
             izz="${mass * (width * width + length * length) / 12 * inertia_scaling}"
             ixy="0" iyz="0" ixz="0"/>
  </xacro:macro>

  <!-- length is along the y-axis! -->
  <xacro:macro name="cylinder_inertia_def" params="radius length mass">
    <inertia ixx="${mass * (3 * radius * radius + length * length) / 12 * inertia_scaling}"
             iyy="${mass * radius* radius / 2 * inertia_scaling}"
             izz="${mass * (3 * radius * radius + length * length) / 12 * inertia_scaling}"
             ixy="0" iyz="0" ixz="0"/>
  </xacro:macro>

  <xacro:property name="arm_elem_link_mass" value="2.0"/>
  <xacro:property name="arm_elem_ball_link_mass" value="2.0"/>
  <xacro:property name="arm_elem_end_link_mass" value="2.0"/>
  <xacro:property name="safety_controller_k_pos" value="100" />
  <xacro:property name="safety_controller_k_vel" value="2" />
  <xacro:property name="joint_damping" value="1" />

  <xacro:property name="arm_velocity_scale_factor" value="1"/>
  <xacro:property name="right" value="0" />
  <!-- right is either 1 (for right arm) or -1 (for left arm) -->
    <link name="world"/>
    <joint name="base_joint" type="fixed">
      <origin xyz="0 0 0.055" rpy="0 0 0"/>
      <axis xyz="0 0 1"/>
      <child link="calib_${name}_arm_base_link"/>
      <parent link="world"/>
    </joint>
<link name ...