<?xml version="1.0" ?>
<!--
  Demonstration of wheel plowing effect with tricycle models.
  Be sure to add the test/models folder to your GAZEBO_MODEL_PATH before
  running this world.

  This world contains a ground plane with gz:plowing_terrain defined
  and two tricycle models with the WheelSlipPlugin that use spherical wheels
  to simplify the contact geometry.
  The trisphere_cycle model does not have plowing enabled, while the
  plowing_effect_trisphere_cycle model does have plowing enabled.
  Each model has a JointControlPlugin that enables a speed controller on each
  wheel to cause the tricycles to drive forward.

  Though the joint controllers are controlling the same wheel speeds, the
  increased drag caused by the plowing effect reduces the linear speed
  that is reached by the plowing_effect_trisphere_cycle model.
  Enable "View Contacts" and "View transparent" or "View Wireframe" in gzclient
  to see the change in contact point location and normal direction due to the
  plowing effect.

  Try adjusting wheel slip parameters or the gz:plowing_wheel parameters in
  test/models/plowing_effect_trisphere_cycle/model.sdf to experiment
  with the plowing effect.
-->
<sdf version="1.6">
  <world name="default">
	<gravity>0 0 -9.75</gravity>
    <include>
      <uri>model://plowing_effect_ground_plane</uri>
    </include>
    <include>
      <uri>model://sun</uri>
    </include>

    <include>
      <uri>model://plowing_effect_trisphere_cycle</uri>
      <name>my_tricycle</name>
      <pose>0 0 0  0 0 0</pose>

      <plugin name="wheel_slip" filename="libWheelSlipPlugin.so">
        <wheel link_name="wheel_front">
		<slip_compliance_lateral>3.0</slip_compliance_lateral>
		<slip_compliance_longitudinal>3.0</slip_compliance_longitudinal>
          <wheel_normal_force>32</wheel_normal_force>
        </wheel>
        <wheel link_name="wheel_rear_left">
		<slip_compliance_lateral>3.0</slip_compliance_lateral>
		<slip_compliance_longitudinal>3.0</slip_compliance_longitudinal>
          <wheel_normal_force>32</wheel_normal_force>
        </wheel>
        <wheel link_name="wheel_rear_right">
		<slip_compliance_lateral>3.0</slip_compliance_lateral>
		<slip_compliance_longitudinal>3.0</slip_compliance_longitudinal>
          <wheel_normal_force>32</wheel_normal_force>
        </wheel>
      </plugin>

      <plugin name="joint_control" filename="libJointControlPlugin.so">
        <controller type="position">
          <joint>wheel_front_steer</joint>
          <target>0</target>
          <pid_gains>9 0 0.1</pid_gains>
        </controller>
        <controller type="velocity">
          <joint>wheel_rear_left_spin</joint>
          <joint>wheel_rear_right_spin</joint>
          <joint>wheel_front_spin</joint>
          <target>6.0</target>
          <pid_gains>9 0 0</pid_gains>
        </controller>
        <controller type="force">
          <joint>wheel_rear_left_spin</joint>
          <joint>wheel_rear_right_spin</joint>
          <target>2.15</target>
        </controller>
      </plugin>

      <!-- enable these plugins if using ros2
      <plugin name="ground_truth" filename="libgazebo_ros_p3d.so">
        <alwaysOn>true</alwaysOn>
        <frameName>world</frameName>
        <body_name>frame</body_name>
        <topicName>odom</topicName>
        <updateRate>30.0</updateRate>
      </plugin>

      <plugin name="wheel_rotation" filename="libgazebo_ros_joint_state_publisher.so">
        <updateRate>30.0</updateRate>
        <joint_name>wheel_rear_left_spin</joint_name>
        <joint_name>wheel_rear_right_spin</joint_name>
      </plugin>
      -->

    </include>

    <include>
      <uri>model://trisphere_cycle</uri>
      <name>original_tricycle</name>
      <pose>0 2 0  0 0 0</pose>

      <plugin name="wheel_slip" filename="libWheelSlipPlugin.so">
        <wheel link_name="wheel_front">
		<slip_compliance_lateral>3.0</slip_compliance_lateral>
		<slip_compliance_longitudinal>3.0</slip_compliance_longitudinal>
          <wheel_normal_force>32</wheel_normal_force>
        </wheel>
        <wheel link_name="wheel_rear_left">
		<slip_compliance_lateral>3.0</slip_compliance_lateral>
		<slip_compliance_longitudinal>3.0</slip_compliance_longitudinal>
          <wheel_normal_force>32</wheel_normal_force>
        </wheel>
        <wheel link_name="wheel_rear_right">
		<slip_compliance_lateral>3.0</slip_compliance_lateral>
		<slip_compliance_longitudinal>3.0</slip_compliance_longitudinal>
          <wheel_normal_force>32</wheel_normal_force>
        </wheel>
      </plugin>

      <plugin name="joint_control" filename="libJointControlPlugin.so">
        <controller type="position">
          <joint>wheel_front_steer</joint>
          <target>0</target>
          <pid_gains>9 0 0.1</pid_gains>
        </controller>
        <controller type="velocity">
          <joint>wheel_rear_left_spin</joint>
          <joint>wheel_rear_right_spin</joint>
          <joint>wheel_front_spin</joint>
          <target>6.0</target>
          <pid_gains>9 0 0</pid_gains>
        </controller>
        <controller type="force">
          <joint>wheel_rear_left_spin</joint>
          <joint>wheel_rear_right_spin</joint>
          <target>2.15</target>
        </controller>
      </plugin>

      <!-- enable these plugins if using ros2
      <plugin name="ground_truth" filename="libgazebo_ros_p3d.so">
        <alwaysOn>true</alwaysOn>
        <frameName>world</frameName>
        <body_name>frame</body_name>
        <topicName>odom</topicName>
        <updateRate>30.0</updateRate>
      </plugin>

      <plugin name="wheel_rotation" filename="libgazebo_ros_joint_state_publisher.so">
        <updateRate>30.0</updateRate>
        <joint_name>wheel_rear_left_spin</joint_name>
        <joint_name>wheel_rear_right_spin</joint_name>
      </plugin>
      -->

    </include>

    <gui fullscreen='0'>
      <camera name='user_camera'>
        <pose>1.5 -4 2.5  0 0.5 1.6</pose>
        <view_controller>orbit</view_controller>
      </camera>
    </gui>
  </world>
</sdf>

