<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <ros2_control name="GazeboSystem" type="system">
    <hardware>
      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
    </hardware>

    <joint name="FR_hip_joint">
      <command_interface name="effort" initial_value="0.0"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="FR_thigh_joint">
      <command_interface name="effort" initial_value="0.0"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="FR_calf_joint">
      <command_interface name="effort" initial_value="0.0"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="FL_hip_joint">
      <command_interface name="effort" initial_value="0.0"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="FL_thigh_joint">
      <command_interface name="effort" initial_value="0.0"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="FL_calf_joint">
      <command_interface name="effort" initial_value="0.0"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="RR_hip_joint">
      <command_interface name="effort" initial_value="0.0"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="RR_thigh_joint">
      <command_interface name="effort" initial_value="0.0"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="RR_calf_joint">
      <command_interface name="effort" initial_value="0.0"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="RL_hip_joint">
      <command_interface name="effort" initial_value="0.0"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="RL_thigh_joint">
      <command_interface name="effort" initial_value="0.0"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <joint name="RL_calf_joint">
      <command_interface name="effort" initial_value="0.0"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

    <sensor name="imu_sensor">
      <state_interface name="orientation.x"/>
      <state_interface name="orientation.y"/>
      <state_interface name="orientation.z"/>
      <state_interface name="orientation.w"/>
      <state_interface name="angular_velocity.x"/>
      <state_interface name="angular_velocity.y"/>
      <state_interface name="angular_velocity.z"/>
      <state_interface name="linear_acceleration.x"/>
      <state_interface name="linear_acceleration.y"/>
      <state_interface name="linear_acceleration.z"/>
    </sensor>
  </ros2_control>

  <gazebo>
    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
      <parameters>$(find cyberdog_description)/config/gazebo.yaml</parameters>
    </plugin>
  </gazebo>

  <gazebo reference="trunk">
    <mu1>0.6</mu1>
    <mu2>0.6</mu2>
    <self_collide>1</self_collide>
  </gazebo>

  <gazebo reference="imu_link">
      <gravity>true</gravity>
      <sensor name="imu_sensor" type="imu">
          <always_on>true</always_on>
          <update_rate>1000</update_rate>
          <visualize>true</visualize>
          <topic>__default_topic__</topic>
          <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
              <topicName>trunk_imu</topicName>
              <bodyName>imu_link</bodyName>
              <updateRateHZ>1000.0</updateRateHZ>
              <gaussianNoise>0.0</gaussianNoise>
              <xyzOffset>0 0 0</xyzOffset>
              <rpyOffset>0 0 0</rpyOffset>
              <frameName>imu_link</frameName>
          </plugin>
          <pose>0 0 0 0 0 0</pose>
      </sensor>
  </gazebo>

  <!-- FL leg -->
  <gazebo reference="FL_abad">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <gazebo reference="FL_hip">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <self_collide>1</self_collide>
    <kp  value="1000000.0"/>
    <kd  value="1.0"/>
  </gazebo>
  <gazebo reference="FL_knee">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <self_collide>1</self_collide>
  </gazebo>
  <gazebo reference="FL_foot">
    <mu1>0.6</mu1>
    <mu2>0.6</mu2>
    <self_collide>1</self_collide>
    <kp  value="1000000.0"/>
    <kd  value="1.0"/>
  </gazebo>

  <!-- FR leg -->
  <gazebo reference="FR_abad">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <gazebo reference="FR_hip">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <self_collide>1</self_collide>
    <kp  value="1000000.0"/>
    <kd  value="1.0"/>
  </gazebo>
  <gazebo reference="FR_knee">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <self_collide>1</self_collide>
  </gazebo>
  <gazebo reference="FR_foot">
    <mu1>0.6</mu1>
    <mu2>0.6</mu2>
    <self_collide>1</self_collide>
    <kp  value="1000000.0"/>
    <kd  value="1.0"/>
  </gazebo>

  <!-- RL leg -->
  <gazebo reference="RL_abad">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <gazebo reference="RL_hip">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <self_collide>1</self_collide>
    <kp  value="1000000.0"/>
    <kd  value="1.0"/>
  </gazebo>
  <gazebo reference="RL_knee">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <self_collide>1</self_collide>
  </gazebo>
  <gazebo reference="RL_foot">
    <mu1>0.6</mu1>
    <mu2>0.6</mu2>
    <self_collide>1</self_collide>
    <kp  value="1000000.0"/>
    <kd  value="1.0"/>
  </gazebo>

  <!-- RR leg -->
  <gazebo reference="RR_abad">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <gazebo reference="RR_hip">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <self_collide>1</self_collide>
    <kp  value="1000000.0"/>
    <kd  value="1.0"/>
  </gazebo>
  <gazebo reference="RR_knee">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <self_collide>1</self_collide>
  </gazebo>
  <gazebo reference="RR_foot">
    <mu1>0.6</mu1>
    <mu2>0.6</mu2>
    <self_collide>1</self_collide>
    <kp  value="1000000.0"/>
    <kd  value="1.0"/>
  </gazebo>
</robot>
