<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="plate">
  
  <!-- <gazebo> -->
  <!--   <self_collide>true</self_collide> -->
  <!-- </gazebo> -->

  <xacro:property name="namespace" value="" />
  <xacro:property name="base_mesh_file" value="base2.dae" />
  <xacro:property name="shaft_mesh_file" value="shaft1.dae" />
  <xacro:property name="plate_r_mesh_file" value="half_plate_r.dae" />
  <xacro:property name="plate_l_mesh_file" value="half_plate_l.dae" />
  <xacro:property name="mesh_scale" value="1 1 1"/>


  <xacro:include filename="$(find wall)/urdf/macro.xacro"/>

  <xacro:link_base name="base_link" mesh_file="${base_mesh_file}" mesh_scale="${mesh_scale}"/>
  
  <xacro:joint_shaft name="joint_shaft_1" parent="base_link" child="shaft_1" origin_xyz="0.055 0 0.065"/>
  <xacro:link_shaft name="shaft_1" mesh_file="${shaft_mesh_file}" mesh_scale="${mesh_scale}" origin_xyz="0 0 0"/>

  <xacro:joint_shaft name="joint_shaft_2" parent="base_link" child="shaft_2" origin_xyz="-0.055 0 0.065"/>
  <xacro:link_shaft name="shaft_2" mesh_file="${shaft_mesh_file}" mesh_scale="${mesh_scale}" origin_xyz="0 0 0"/>
 
  <xacro:joint_plate name="ball_1" parent="shaft_1" child="plate_1" link_1="ball_link_11" link_2="ball_link_12" origin_xyz="0 0 0.12" rotation_1="0 1 0" rotation_2="1 0 0"/>  
  <xacro:link_ball name="ball_link_11"/> 
  <xacro:link_ball name="ball_link_12"/>
  <xacro:link_plate name="plate_1" mesh_file="${plate_l_mesh_file}" mesh_scale="${mesh_scale}" origin_xyz="0 0 0" rotation="0 0 0" />

  <xacro:joint_plate name="ball_2" parent="shaft_2" child="plate_2" link_1="ball_link_21" link_2="ball_link_22" origin_xyz="0 0 0.12" rotation_1="0 1 0" rotation_2="1 0 0"/>  
  <xacro:link_plate name="plate_2" mesh_file="${plate_r_mesh_file}" mesh_scale="${mesh_scale}" origin_xyz="0 0 0" rotation="0 0 0"/>
  <xacro:link_ball name="ball_link_21"/> 
  <xacro:link_ball name="ball_link_22"/>   

  <gazebo>
    <joint name="joint_connecting_plates" type="fixed">
      <pose>0 0 0 0 0 0</pose>
      <parent>plate_1</parent>
      <child>plate_2</child>
    </joint>
  </gazebo>

  <!-- <gazebo reference="joint_shaft_1"> -->
  <!--   <provideFeedback>true</provideFeedback> -->
  <!-- </gazebo> -->

  <!-- The ft_sensor plugin -->  
  <!-- <gazebo> -->
  <!--     <plugin name="ft_sensor" filename="libgazebo_ros_ft_sensor.so"> -->
  <!--         <updateRate>100.0</updateRate> -->
  <!--         <topicName>ft_sensor_topic</topicName> -->
  <!--         <jointName>joint_shaft_1</jointName> -->
  <!--     </plugin> -->
  <!-- </gazebo> -->

  <gazebo reference="joint_shaft_1">
    <implicitSpringDamper>false</implicitSpringDamper>
    <springStiffness>300</springStiffness>
    <springReference>0</springReference>
  </gazebo>

  <gazebo reference="joint_shaft_2">
    <implicitSpringDamper>false</implicitSpringDamper>
    <springStiffness>300</springStiffness>
    <springReference>0</springReference>
  </gazebo>

</robot>
