Hello everyone,

I am working with "ros2 (humble)" and trying to simulate openmanipulator arm in Gazebo. I can spawn it in gazebo, but when I include "gazeboros2_controller", it keeps moving everywhere like shown in this photo. C:\fakepath\Noworldjoint-min.png. It keeps rotating. Sometimes it flies in the air as well. I tried introducing a fixed joint (I thought it might stop rotating if I do so). When I did that, as you can see in the screenshot, the base link stays fixed to ground, other links start flying.C:\fakepath\Fixedworldjoint-min.png. I dont know if this the problem related to gazebo or I am giving some false input. The URDF file that I am using:

Link and Joints part of URDF:

<?xml version="1.0"?>
<!-- Open_Manipulator Chain -->
<!-- <robot xmlns:xacro="http://ros.org/wiki/xacro"> -->
<robot name="open_manipulator" xmlns:xacro="http://ros.org/wiki/xacro">

  <!-- Link 1 -->
  <link name="link1">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="file://$(find open_manipulator_wv_moveit_config)/meshes/chain_link1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="grey"/>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="file://$(find open_manipulator_wv_moveit_config)/meshes/chain_link1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="3.0876154e-04 0.0000000e+00 -1.2176461e-04" />
      <mass value="7.9119962e-02" />
      <inertia ixx="1.2505234e-05" ixy="0.0" ixz="-1.7855208e-07"
               iyy="2.1898364e-05" iyz="0.0"
               izz="1.9267361e-05" />
    </inertial>
  </link>

  <!-- Joint 1 -->
  <joint name="joint1" type="revolute">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="0.012 0.0 0.017" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit velocity="4.8" effort="1" lower="${-pi*0.9}" upper="${pi*0.9}" />
  </joint>

  <!--  Link 2 -->
  <link name="link2">
    <visual>
      <origin xyz="0 0 0.019" rpy="0 0 0"/>
      <geometry>
        <mesh filename="file://$(find open_manipulator_wv_moveit_config)/meshes/chain_link2.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="grey"/>
    </visual>

    <collision>
      <origin xyz="0 0 0.019" rpy="0 0 0"/>
      <geometry>
        <mesh filename="file://$(find open_manipulator_wv_moveit_config)/meshes/chain_link2.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="-3.0184870e-04 5.4043684e-04 ${0.018 + 2.9433464e-02}" />
      <mass value="9.8406837e-02" />
      <inertia ixx="3.4543422e-05" ixy="-1.6031095e-08" ixz="-3.8375155e-07"
               iyy="3.2689329e-05" iyz="2.8511935e-08"
               izz="1.8850320e-05" />
    </inertial>
  </link>

  <!--  Joint 2 -->
  <joint name="joint2" type="revolute">
    <parent link="link2"/>
    <child link="link3"/>
    <origin xyz="0.0 0.0 0.0595" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit velocity="4.8" effort="1" lower="${-pi*0.57}" upper="${pi*0.5}" />
  </joint>

  <!--  Link 3 -->
  <link name="link3">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="file://$(find open_manipulator_wv_moveit_config)/meshes/chain_link3.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="grey"/>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="file://$(find open_manipulator_wv_moveit_config)/meshes/chain_link3.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="1.0308393e-02 3.7743363e-04 1.0170197e-01" />
      <mass value="1.3850917e-01" />
      <inertia ixx="3.3055381e-04" ixy="-9.7940978e-08" ixz="-3.8505711e-05"
               iyy="3.4290447e-04" iyz="-1.5717516e-06"
               izz="6.0346498e-05" />
    </inertial>
  </link>

  <!--  Joint 3 -->
  <joint name="joint3" type="revolute">
    <parent link="link3"/>
    <child link="link4"/>
    <origin xyz="0.024 0 0.128" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit velocity="4.8" effort="1" lower="${-pi*0.3}" upper="${pi*0.44}" />
  </joint>

  <!--  Link 4 -->
  <link name="link4">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="file://$(find open_manipulator_wv_moveit_config)/meshes/chain_link4.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="grey"/>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="file://$(find open_manipulator_wv_moveit_config)/meshes/chain_link4.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="9.0909590e-02 3.8929816e-04 2.2413279e-04" />
      <mass value="1.3274562e-01" />
      <inertia ixx="3.0654178e-05" ixy="-1.2764155e-06" ixz="-2.6874417e-07"
               iyy="2.4230292e-04" iyz="1.1559550e-08"
               izz="2.5155057e-04" />
    </inertial>
  </link>

  <!--  Joint 4 -->
  <joint name="joint4" type="revolute">
    <parent link="link4"/>
    <child link="link5"/>
    <origin xyz="0.124 0.0 0.0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit velocity="4.8" effort="1" lower="${-pi*0.57}" upper="${pi*0.65}" />
  </joint>

  <!--  Link 5 -->
  <link name="link5">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="file://$(find open_manipulator_wv_moveit_config)/meshes/chain_link5.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="grey"/>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="file://$(find open_manipulator_wv_moveit_config)/meshes/chain_link5.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="4.4206755e-02 3.6839985e-07 8.9142216e-03" />
      <mass value="1.4327573e-01" />
      <inertia ixx="8.0870749e-05" ixy="0.0" ixz="-1.0157896e-06"
               iyy="7.5980465e-05" iyz="0.0"
               izz="9.3127351e-05" />
    </inertial>
  </link>

  <!--  Gripper joint -->
  <joint name="gripper" type="prismatic">
    <parent link="link5"/>
    <child link="gripper_link"/>
    <origin xyz="0.0817 0.021 0.0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit velocity="4.8" effort="1" lower="-0.010" upper="0.019" />
  </joint>

  <!--  Gripper link -->
  <link name="gripper_link">
    <visual>
      <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="file://$(find open_manipulator_wv_moveit_config)/meshes/chain_link_grip_l.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="grey"/>
    </visual>

    <collision>
      <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="file://$(find open_manipulator_wv_moveit_config)/meshes/chain_link_grip_l.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0" />
      <mass value="1.0e-03" />
      <inertia ixx="1.0e-03" ixy="0.0" ixz="0.0"
               iyy="1.0e-03" iyz="0.0"
               izz="1.0e-03" />
    </inertial>
  </link>

  <!--  Gripper joint sub -->
  <joint name="gripper_sub" type="prismatic">
    <parent link="link5"/>
    <child link="gripper_link_sub"/>
    <origin xyz="0.0817 -0.021 0" rpy="0 0 0"/>
    <axis xyz="0 -1 0"/>
    <limit velocity="4.8" effort="1" lower="-0.010" upper="0.019" />
    <mimic joint="gripper" multiplier="1"/>
  </joint>

  <!--  Gripper link sub -->
  <link name="gripper_link_sub">
    <visual>
      <origin xyz="0.0 -0.0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="file://$(find open_manipulator_wv_moveit_config)/meshes/chain_link_grip_r.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="grey"/>
    </visual>

    <collision>
      <origin xyz="0.0 -0.0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="file://$(find open_manipulator_wv_moveit_config)/meshes/chain_link_grip_r.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0" />
      <mass value="1.0e-03" />
      <inertia ixx="1.0e-03" ixy="0.0" ixz="0.0"
               iyy="1.0e-03" iyz="0.0"
               izz="1.0e-03" />
    </inertial>
  </link>

  <!-- end effector joint -->
  <joint name="end_effector_joint" type="fixed">
    <origin xyz="0.126 0.0 0.0" rpy="0 0 0"/>
    <parent link="link5"/>
    <child link="end_effector_link"/>
  </joint>

  <!-- end effector link -->
  <link name="end_effector_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.01 0.01 0.01" />
      </geometry>
      <material name="red"/>
     </visual>

    <inertial>
      <origin xyz="0 0 0" />
      <mass value="1.0e-03" />
      <inertia ixx="1.0e-03" ixy="0.0" ixz="0.0"
               iyy="1.0e-03" iyz="0.0"
               izz="1.0e-03" />
    </inertial>
  </link>

  <!-- Realsense Camera Joint Position -->
  <joint name="camera_joint" type="fixed">
    <axis xyz="0 1 0" />
    <origin xyz="0.070 0.032 0.052" rpy="0 0 0"/> 
    <parent link="link5"/>
    <child link="camera_link"/>
  </joint>

  <link name="camera_link"/> 

  <!-- World -->
  <link name="world"/>

  <!-- World fixed joint-->
  <joint name="world_fixed" type="fixed">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <parent link="world"/>
    <child link="link1"/>
  </joint>

ros2_controller part of URDF:

<ros2_control name="GazeboSimSystem" type="system">
    <hardware>
        <plugin>gazebo_ros2_control/GazeboSystem</plugin>
    </hardware>
    <joint name="joint1">
      <command_interface name="position">
        <param name="initial_value">0</param>
        <param name="min">-2.826</param> <!-- ** -3.935 -->
        <param name="max">2.826</param>
      </command_interface>
      <state_interface name="position">
        <param name="initial_value">0</param>
      </state_interface>
      <state_interface name="velocity"/>
    </joint>
    <joint name="joint2">
      <command_interface name="position">
        <param name="initial_value">0</param>
        <param name="min">-1.7898</param> <!-- ** -3.935 -->
        <param name="max">1.6878</param>
      </command_interface>
      <state_interface name="position">
        <param name="initial_value">0</param>
      </state_interface>
      <state_interface name="velocity"/>
    </joint>
    <joint name="joint3">
      <command_interface name="position">
        <param name="initial_value">0</param>
        <param name="min">-0.942</param> <!-- ** -3.935 -->
        <param name="max">1.3816</param>
      </command_interface>
      <state_interface name="position">
        <param name="initial_value">0</param>
      </state_interface>
      <state_interface name="velocity"/>
    </joint>
    <joint name="joint4">
      <command_interface name="position">
        <param name="initial_value">0</param>
        <param name="min">-1.7898</param> <!-- ** -3.935 -->
        <param name="max">2.041</param>
      </command_interface>
      <state_interface name="position">
        <param name="initial_value">0</param>
      </state_interface>
      <state_interface name="velocity"/>
    </joint>
    <joint name="gripper">
      <command_interface name="position">
        <param name="initial_value">0</param>
        <param name="min">-0.010</param> <!-- ** -3.935 -->
        <param name="max">0.019</param>
      </command_interface>
      <state_interface name="position">
        <param name="initial_value">0</param>
      </state_interface>
      <state_interface name="velocity"/>
    </joint>
  </ros2_control>

  <gazebo>
        <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
          <robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
          <parameters>$(find open_manipulator_wv_moveit_config)/config/ros2_controllers.yaml</parameters>
        </plugin>
  </gazebo>
</robot>

Is there any problem with the gazebo controller code?

My "ros2_controller.yaml":

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz

    arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController


    hand_controller:
      type: position_controllers/GripperActionController


    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

arm_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
hand_controller:
  ros__parameters:
    joint: gripper

The launch file that I am using is here. C:\fakepath\gazebo_simulation.launch.py

Am I missing something here? Can anyone please guide me? I am trying since three days without any luck.