<?xml version="1.0" ?>
<robot name="iisy" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:property name="joint_damping" value="10.0"/>
    <xacro:property name="joint_friction" value="0.1"/>

    <link name="world"/>
    <link name="base_link"/>
    <link name="table">
        <collision>
            <geometry>
                <mesh filename="file://$(find iisy_config)/stl/table.stl"/>
            </geometry>
        </collision>
        <visual>
            <geometry>
                <mesh filename="file://$(find iisy_config)/stl/table.stl"/>
            </geometry>
        </visual>
    </link>
    <link name="base_bottom">
        <collision>
            <geometry>
                <mesh filename="file://$(find iisy_config)/stl/base_bottom_coll.stl"/>
            </geometry>
        </collision>
        <visual>
            <geometry>
                <mesh filename="file://$(find iisy_config)/stl/base_bottom_low.stl"/>
            </geometry>
        </visual>

        <inertial>
            <origin xyz="-0.043517 0.000000 0.054914" rpy="0 0 0"/>
            <mass value="6"/>
            <inertia ixx="1.277905" ixy="0.0" ixz="0.138279" iyy="3.070705" iyz="0.0" izz="3.284356"/>
        </inertial>
    </link>
    <link name="base_top">
        <collision>
            <origin xyz="0 -0.005 0.08"/>
            <geometry>
                <box size="0.14 0.15 0.16"/>
            </geometry>
        </collision>
        <visual>
            <geometry>
                <mesh filename="file://$(find iisy_config)/stl/base_top_low.stl"/>
            </geometry>
        </visual>

        <inertial>
            <origin xyz="0 -0.005 0.08" rpy="0 0 0"/>
            <mass value="2"/>
            <inertia ixx="0.32666666666667" ixy="0.0" ixz="0.0" iyy="0.32666666666667" iyz="0.0" izz="0.25"/>
        </inertial>

    </link>
    <link name="link1_full">
        <collision>
            <geometry>
                <mesh filename="file://$(find iisy_config)/stl/link_1_full_coll.stl"/>
            </geometry>
        </collision>
        <visual>
            <geometry>
                <mesh filename="file://$(find iisy_config)/stl/link_1_full_low.stl"/>
            </geometry>
        </visual>

        <inertial>
            <origin xyz="0 -0.050000 0.150000" rpy="0 0 0"/>
            <mass value="4"/>
            <inertia ixx="8.266347" ixy="0.0" ixz="0.0" iyy="8.647519" iyz="0.0" izz="8.647519"/>
        </inertial>

    </link>
    <link name="link2_bottom">
        <collision>
            <origin xyz="0 0.07 0.025"/>
            <geometry>
                <box size="0.14 0.14 0.19"/>
            </geometry>
        </collision>
        <visual>
            <geometry>
                <mesh filename="file://$(find iisy_config)/stl/link_2_bottom_low.stl"/>
            </geometry>
        </visual>

        <inertial>
            <origin xyz="0 0.07 0.025" rpy="0 0 0"/>
            <mass value="2"/>
            <inertia ixx="0.5" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="0.25"/>
        </inertial>

    </link>
    <link name="link2_top">
        <collision>
            <geometry>
                <mesh filename="file://$(find iisy_config)/stl/link_2_top_coll.stl"/>
            </geometry>
        </collision>
        <visual>
            <geometry>
                <mesh filename="file://$(find iisy_config)/stl/link_2_top_low.stl"/>
            </geometry>
        </visual>

        <inertial>
            <origin xyz="0 0.037363 0.086674" rpy="0 0 0"/>
            <mass value="2"/>
            <inertia ixx="1.431738" ixy="0.0" ixz="0.0" iyy="1.131426" iyz="-0.342192" izz="0.807202"/>
        </inertial>

    </link>
    <link name="link3_bottom">
        <collision>
            <geometry>
                <mesh filename="file://$(find iisy_config)/stl/link_3_bottom_coll.stl"/>
            </geometry>
        </collision>
        <visual>
            <geometry>
                <mesh filename="file://$(find iisy_config)/stl/link_3_bottom_low.stl"/>
            </geometry>
        </visual>

        <inertial>
            <origin xyz="0 -0.055000 0.021413" rpy="0 0 0"/>
            <mass value="2"/>
            <inertia ixx="0.362124" ixy="0.0" ixz="0.0" iyy="0.343531" iyz="0.0" izz="0.283368"/>
        </inertial>

    </link>
    <link name="link3_top">
        <collision>
            <geometry>
                <mesh filename="file://$(find iisy_config)/stl/link_3_top_coll.stl"/>
            </geometry>
        </collision>
        <visual>
            <geometry>
                <mesh filename="file://$(find iisy_config)/stl/link_3_top_low.stl"/>
            </geometry>
        </visual>

        <inertial>
            <origin xyz="0 -0.015462 0.040000" rpy="0 0 0"/>
            <mass value="0.8"/>
            <inertia ixx="0.228470" ixy="0.0" ixz="0.0" iyy="0.137641" iyz="0.0" izz="0.252003"/>
        </inertial>
    </link>

    <joint type="fixed" name="world_base_link_fixed">
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <parent link="world"/>
        <child link="base_link"/>
    </joint>
    <joint type="fixed" name="base_base_link_fixed">
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="table"/>
    </joint>
    <joint type="fixed" name="table_link_fixed">
        <origin xyz="0 0 1" rpy="0 0 0"/>
        <parent link="table"/>
        <child link="base_bottom"/>
    </joint>
    <joint type="continuous" name="base_rot">
        <origin xyz="0 0 0.1205" rpy="0 0 0"/>
        <parent link="base_bottom"/>
        <child link="base_top"/>
        <axis xyz="0 0 1"/>
        <limit effort="30" velocity="1.7453292519943"/>
        <dynamics damping="${joint_damping}" friction="${joint_friction}"/>
    </joint>
    <joint type="revolute" name="base_link1_joint">
        <origin xyz="0 -0.080499 0.092997" rpy="0 0 0"/>
        <parent link="base_top"/>
        <child link="link1_full"/>
        <axis xyz="0 1 0"/>
        <limit effort="30" velocity="1.7453292519943" lower="-2.2689280275926" upper="2.4434609527921"/>
        <dynamics damping="${joint_damping}" friction="${joint_friction}"/>
    </joint>
    <joint type="revolute" name="link1_link2_joint">
        <origin xyz="0 0 0.3" rpy="0 0 0"/>
        <parent link="link1_full"/>
        <child link="link2_bottom"/>
        <axis xyz="0 1 0"/>
        <limit effort="30" velocity="1.7453292519943" lower="-2.6179938779915" upper="2.6179938779915"/>
        <dynamics damping="${joint_damping}" friction="${joint_friction}"/>
    </joint>
    <joint type="continuous" name="link2_rot">
        <origin xyz="0 0.080501 0.120502" rpy="0 0 0"/>
        <parent link="link2_bottom"/>
        <child link="link2_top"/>
        <axis xyz="0 0 1"/>
        <limit effort="30" velocity="1.7453292519943"/>
        <dynamics damping="${joint_damping}" friction="${joint_friction}"/>
    </joint>
    <joint type="continuous" name="link2_link3_joint">
        <origin xyz="0 0.0565 0.178003" rpy="0 0 0"/>
        <parent link="link2_top"/>
        <child link="link3_bottom"/>
        <axis xyz="0 1 0"/>
        <limit effort="30" velocity="1.7453292519943"/>
        <dynamics damping="${joint_damping}" friction="${joint_friction}"/>
    </joint>
    <joint type="continuous" name="link3_rot">
        <origin xyz="0 -0.055001 0.085501" rpy="0 0 0"/>
        <parent link="link3_bottom"/>
        <child link="link3_top"/>
        <axis xyz="0 0 1"/>
        <limit effort="30" velocity="1.7453292519943"/>
        <dynamics damping="${joint_damping}" friction="${joint_friction}"/>
    </joint>

    <transmission name="trans_base_rot">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="base_rot">
            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
        </joint>
        <actuator name="base_rot_motor">
            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_base_link1_joint">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="base_link1_joint">
            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
        </joint>
        <actuator name="base_link1_joint_motor">
            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_link1_link2_joint">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="link1_link2_joint">
            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
        </joint>
        <actuator name="link1_link2_joint_motor">
            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_link2_rot">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="link2_rot">
            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
        </joint>
        <actuator name="link2_rot_motor">
            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_link2_link3_joint">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="link2_link3_joint">
            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
        </joint>
        <actuator name="link2_link3_joint_motor">
            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_link3_rot">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="link3_rot">
            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
        </joint>
        <actuator name="link3_rot_motor">
            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>

    <ros2_control name="lbr_hardware_interface" type="system">
        <!-- define hardware including parameters, also gazebo -->
        <hardware>
            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
        </hardware>

        <!-- define lbr specific state interfaces as sensor, see https://github.com/ros-controls/roadmap/blob/master/design_drafts/components_architecture_and_urdf_examples.md -->
        <sensor name="iisy_state">
            <!-- see KUKA::FRI::LBRState -->
            <state_interface name="sample_time"/>
            <state_interface name="time_stamp_sec"/>
            <state_interface name="time_stamp_nano_sec"/>
        </sensor>
        <!-- define joints and command/state interfaces for each joint -->
        <joint name="base_rot">
            <command_interface name="position">
                <!-- better to use radians as min max first -->
                <param name="min">-1</param>
                <param name="max"> 1</param>
            </command_interface>
            <command_interface name="effort">
                <param name="min">-1</param>
                <param name="max"> 1</param>
            </command_interface>
            <state_interface name="position"/>
            <state_interface name="effort"/>
            <state_interface name="external_torque"/>
        </joint>
        <joint name="base_link1_joint">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max"> 1</param>
            </command_interface>
            <command_interface name="effort">
                <param name="min">-1</param>
                <param name="max"> 1</param>
            </command_interface>
            <state_interface name="position"/>
            <state_interface name="effort"/>
            <state_interface name="external_torque"/>
        </joint>
        <joint name="link1_link2_joint">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max"> 1</param>
            </command_interface>
            <command_interface name="effort">
                <param name="min">-1</param>
                <param name="max"> 1</param>
            </command_interface>
            <state_interface name="position"/>
            <state_interface name="effort"/>
            <state_interface name="external_torque"/>
        </joint>
        <joint name="link2_rot">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max"> 1</param>
            </command_interface>
            <command_interface name="effort">
                <param name="min">-1</param>
                <param name="max"> 1</param>
            </command_interface>
            <state_interface name="position"/>
            <state_interface name="effort"/>
            <state_interface name="external_torque"/>
        </joint>
        <joint name="link2_link3_joint">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max"> 1</param>
            </command_interface>
            <command_interface name="effort">
                <param name="min">-1</param>
                <param name="max"> 1</param>
            </command_interface>
            <state_interface name="position"/>
            <state_interface name="effort"/>
            <state_interface name="external_torque"/>
        </joint>
        <joint name="link3_rot">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max"> 1</param>
            </command_interface>
            <command_interface name="effort">
                <param name="min">-1</param>
                <param name="max"> 1</param>
            </command_interface>
            <state_interface name="position"/>
            <state_interface name="effort"/>
            <state_interface name="external_torque"/>
        </joint>
    </ros2_control>

    <gazebo>
        <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
            <parameters>$(find iisy_config)/config/iisy_controllers.yml</parameters>
            <robotNamespace>/iisy</robotNamespace>
        </plugin>
    </gazebo>

    <gazebo reference="base_bottom">
        <mu1>0.2</mu1>
        <mu2>0.2</mu2>
        <material>Gazebo/Orange</material>
    </gazebo>

    <gazebo reference="base_top">
        <mu1>0.2</mu1>
        <mu2>0.2</mu2>
        <material>Gazebo/Orange</material>
    </gazebo>

    <gazebo reference="link1_full">
        <mu1>0.2</mu1>
        <mu2>0.2</mu2>
        <material>Gazebo/Orange</material>
    </gazebo>

    <gazebo reference="link2_bottom">
        <mu1>0.2</mu1>
        <mu2>0.2</mu2>
        <material>Gazebo/Orange</material>
    </gazebo>

    <gazebo reference="link2_top">
        <mu1>0.2</mu1>
        <mu2>0.2</mu2>
        <material>Gazebo/Orange</material>
    </gazebo>

    <gazebo reference="link3_bottom">
        <mu1>0.2</mu1>
        <mu2>0.2</mu2>
        <material>Gazebo/Orange</material>
    </gazebo>
    
    <gazebo reference="link3_top">
        <mu1>0.2</mu1>
        <mu2>0.2</mu2>
        <material>Gazebo/Grey</material>
    </gazebo>

</robot>