<?xml version="1.0" ?>
<sdf version="1.5">
<model name="husky">
<!-- old values left for reference -->
<!--pose>0 0 .5 0 0 0</pose-->
<pose>0 0 0 0 0 0</pose>
<!-- Main Chassis -->
<link name="base_link">
<!-- Physics -->
<collision name="collision">
<!-- <pose>0 0 -0.3 0 0 0</pose> -->
<pose>0 0 0.1 0 0 0</pose>
<geometry>
<box>
<size>1.0074 0.5709 0.2675</size>
</box>
</geometry>
</collision>
<inertial>
<mass>33.855</mass>
<pose>-0.0856132 -0.000839955 0.238145 0 0 0</pose>
<inertia>
<ixx>2.2343</ixx>
<ixy>-0.023642</ixy>
<ixz>0.275174</ixz>
<iyy>3.42518</iyy>
<iyz>0.00239624</iyz>
<izz>2.1241</izz>
</inertia>
</inertial>
<!-- Visual -->
<!-- Base frame -->
<visual name="body">
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<uri>model://husky_model/meshes/body.dae</uri>
</mesh>
</geometry>
</visual>
</link>
<!-- Back Left Wheel -->
<link name="back_left_wheel">
<pose>-0.256 0.285475 0.035 0 0 0</pose>
<inertial>
<mass>2.6357</mass>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>0.0246688</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0246688</iyy>
<iyz>0</iyz>
<izz>0.0441058</izz>
</inertia>
</inertial>
<collision name="back_left_wheel_collision">
<pose>0 0 0 -1.5707 0 0</pose>
<geometry>
<cylinder>
<radius>0.17775</radius>
<length>0.1143</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>100000.0</mu>
<mu2>100000.0</mu2>
<slip1>0.0</slip1>
<slip2>0.0</slip2>
</ode>
</friction>
</surface>
</collision>
<visual name="back_left_wheel">
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<uri>model://husky_model/meshes/wheel.dae</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name="back_left_joint" type="revolute">
<sensor name="back_left_sensor" type="force_torque"><force_torque></force_torque></sensor>
<parent>base_link</parent>
<child>back_left_wheel</child>
<axis>
<xyz>0 1 0</xyz>
<use_parent_model_frame>true</use_parent_model_frame>
</axis>
<physics>
<provide_feedback>true</provide_feedback>
<ode>
<limit>
<cfm>0.000000</cfm>
<erp>0.900000</erp>
</limit>
</ode>
</physics>
</joint>
<!-- Back Right Wheel -->
<link name="back_right_wheel">
<pose>-0.256 -0.285475 0.035 0 0 0</pose>
<inertial>
<mass>2.6357</mass>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>0.0246688</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0246688</iyy>
<iyz>0</iyz>
<izz>0.0441058</izz>
</inertia>
</inertial>
<collision name="back_right_wheel_collision">
<pose>0 0 0 -1.5707 0 0</pose>
<geometry>
<cylinder>
<radius>0.17775</radius>
<length>0.1143</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>100000.0</mu>
<mu2>100000.0</mu2>
<slip1>0.0</slip1>
<slip2>0.0</slip2>
</ode>
</friction>
</surface>
</collision>
<visual name="back_right_wheel">
<pose>0 0 0 -3.14159 0 0</pose>
<geometry>
<mesh>
<uri>model://husky_model/meshes/wheel.dae</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name="back_right_joint" type="revolute">
<sensor name="back_right_sensor" type="force_torque"><force_torque></force_torque></sensor>
<parent>base_link</parent>
<child>back_right_wheel</child>
<axis>
<xyz>0 1 0</xyz>
<use_parent_model_frame>true</use_parent_model_frame>
</axis>
<physics>
<provide_feedback>true</provide_feedback>
<ode>
<limit>
<cfm>0.000000</cfm>
<erp>0.900000</erp>
</limit>
</ode>
</physics>
</joint>
<!-- Front Left Wheel -->
<link name="front_left_wheel">
<pose>0.256 0.285475 0.035 0 0 0</pose>
<inertial>
<mass>2.6357</mass>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>0.0246688</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0246688</iyy>
<iyz>0</iyz>
<izz>0.0441058</izz>
</inertia>
</inertial>
<collision name="front_left_wheel_collision">
<pose>0 0 0 -1.5707 0 0</pose>
<geometry>
<cylinder>
<radius>0.17775</radius>
<length>0.1143</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>100000.0</mu>
<mu2>100000.0</mu2>
<slip1>0.0</slip1>
<slip2>0.0</slip2>
</ode>
</friction>
</surface>
</collision>
<visual name="front_left_wheel">
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<uri>model://husky_model/meshes/wheel.dae</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name="front_left_joint" type="revolute">
<sensor name="front_left_sensor" type="force_torque"><force_torque></force_torque></sensor>
<parent>base_link</parent>
<child>front_left_wheel</child>
<axis>
<xyz>0 1 0</xyz>
<use_parent_model_frame>true</use_parent_model_frame>
</axis>
<physics>
<provide_feedback>true</provide_feedback>
<ode>
<limit>
<cfm>0.000000</cfm>
<erp>0.900000</erp>
</limit>
</ode>
</physics>
</joint>
<!-- Front Right Wheel -->
<link name="front_right_wheel">
<pose>0.256 -0.285475 0.035 0 0 0</pose>
<inertial>
<mass>2.6357</mass>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>0.0246688</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0246688</iyy>
<iyz>0</iyz>
<izz>0.0441058</izz>
</inertia>
</inertial>
<collision name="front_right_wheel_collision">
<pose>0 0 0 -1.5707 0 0</pose>
<geometry>
<cylinder>
<radius>0.17775</radius>
<length>0.1143</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>100000.0</mu>
<mu2>100000.0</mu2>
<slip1>0.0</slip1>
<slip2>0.0</slip2>
</ode>
</friction>
</surface>
</collision>
<visual name="front_right_wheel">
<pose>0 0 0 -3.14159 0 0</pose>
<geometry>
<mesh>
<uri>model://husky_model/meshes/wheel.dae</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name="front_right_joint" type="revolute">
<sensor name="front_right_sensor" type="force_torque"><force_torque></force_torque></sensor>
<parent>base_link</parent>
<child>front_right_wheel</child>
<axis>
<xyz>0 1 0</xyz>
<use_parent_model_frame>true</use_parent_model_frame>
</axis>
<physics>
<provide_feedback>true</provide_feedback>
<ode>
<limit>
<cfm>0.000000</cfm>
<erp>0.900000</erp>
</limit>
</ode>
</physics>
</joint>
<!-- eye vision camera -->
<link name="eye_vision_camera">
<pose>-0.16 0 0.23 0 0 0</pose>
<!-- <inertial>
<mass>0.85</mass>
<pose>-0.000001 0.090907 -0.000321 0 0 0</pose>
<inertia>
<ixx>0.00885564</ixx>
<ixy>-0.00000001</ixy>
<ixz>-0.00000003</ixz>
<iyy>0.00369008</iyy>
<iyz>0.00016932</iyz>
<izz>0.00898176</izz>
</inertia>
</inertial> -->
<collision name="eye_vision_camera_collision">
<pose>0 0 0.08 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.05</radius>
<length>0.15</length>
</cylinder>
</geometry>
<!-- <surface>
<friction>
<ode>
<mu>100000.0</mu>
<mu2>100000.0</mu2>
<slip1>0.0</slip1>
<slip2>0.0</slip2>
</ode>
</friction>
</surface> -->
</collision>
<visual name="eye_vision_camera">
<pose>0 0 0 1.570796327 0 -1.570796327</pose>
<geometry>
<mesh>
<uri>model://husky_model/meshes/eye_vision_camera.dae</uri>
</mesh>
</geometry>
</visual>
<sensor name="camera" type="camera">
<pose>0 0 0.132 0 0 0</pose>
<topic>/husky/camera</topic>
<camera>
<horizontal_fov>1.57</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
</camera>
<always_on>1</always_on>
<update_rate>30</update_rate>
<visualize>true</visualize>
<plugin name="mouse_right_eye" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>30.0</updateRate>
<cameraName>husky</cameraName>
<imageTopicName>camera</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</link>
<joint name="eye_vision_camera_joint" type="revolute">
<parent>base_link</parent>
<child>eye_vision_camera</child>
<axis>
<xyz>0 0 1</xyz>
<use_parent_model_frame>true</use_parent_model_frame>
<limit>
<lower>0.000000</lower>
<upper>0.000000</upper>
</limit>
</axis>
</joint>
<plugin name="husky_diff_controller" filename="libhusky_gazebo_plugins.so">
<alwaysOn>true</alwaysOn>
<updateRate>100.0</updateRate>
<backLeftJoint>back_left_joint</backLeftJoint>
<backRightJoint>back_right_joint</backRightJoint>
<frontLeftJoint>front_left_joint</frontLeftJoint>
<frontRightJoint>front_right_joint</frontRightJoint>
<wheelSeparation>0.5709</wheelSeparation>
<wheelDiameter>0.3555</wheelDiameter>
<torque>35</torque>
</plugin>
</model>
</sdf>