<?xml version="1.0" ?>
<robot name="fr3">
	<link name="fr3_link0">
		<visual>
			<geometry>
				<mesh filename="package://franka_description/meshes/visual/link0.dae"/>
			</geometry>
		</visual>
		<collision>
			<origin rpy="0 1.5707963267948966 0" xyz="-0.075 0 0.06"/>
			<geometry>
				<cylinder length="0.03" radius="0.09"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="-0.06 0 0.06"/>
			<geometry>
				<sphere radius="0.09"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="-0.09 0 0.06"/>
			<geometry>
				<sphere radius="0.09"/>
			</geometry>
		</collision>
	</link>
	<link name="fr3_link1">
		<visual>
			<geometry>
				<mesh filename="package://franka_description/meshes/visual/link1.dae"/>
			</geometry>
		</visual>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 -0.1915"/>
			<geometry>
				<cylinder length="0.2830" radius="0.09"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 -0.333"/>
			<geometry>
				<sphere radius="0.09"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 -0.05"/>
			<geometry>
				<sphere radius="0.09"/>
			</geometry>
		</collision>
	</link>
	<joint name="fr3_joint1" type="revolute">
		<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
		<origin rpy="0 0 0" xyz="0 0 0.333"/>
		<parent link="fr3_link0"/>
		<child link="fr3_link1"/>
		<axis xyz="0 0 1"/>
		<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
	</joint>
	<link name="fr3_link2">
		<visual>
			<geometry>
				<mesh filename="package://franka_description/meshes/visual/link2.dae"/>
			</geometry>
		</visual>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 0"/>
			<geometry>
				<cylinder length="0.12" radius="0.09"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 -0.06"/>
			<geometry>
				<sphere radius="0.09"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 0.06"/>
			<geometry>
				<sphere radius="0.09"/>
			</geometry>
		</collision>
	</link>
	<joint name="fr3_joint2" type="revolute">
		<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
		<origin rpy="-1.5707963267948966 0 0" xyz="0 0 0"/>
		<parent link="fr3_link1"/>
		<child link="fr3_link2"/>
		<axis xyz="0 0 1"/>
		<limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
	</joint>
	<link name="fr3_link3">
		<visual>
			<geometry>
				<mesh filename="package://franka_description/meshes/visual/link3.dae"/>
			</geometry>
		</visual>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 -0.145"/>
			<geometry>
				<cylinder length="0.15" radius="0.09"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 -0.22"/>
			<geometry>
				<sphere radius="0.09"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 -0.07"/>
			<geometry>
				<sphere radius="0.09"/>
			</geometry>
		</collision>
	</link>
	<joint name="fr3_joint3" type="revolute">
		<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
		<origin rpy="1.5707963267948966 0 0" xyz="0 -0.316 0"/>
		<parent link="fr3_link2"/>
		<child link="fr3_link3"/>
		<axis xyz="0 0 1"/>
		<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
	</joint>
	<link name="fr3_link4">
		<visual>
			<geometry>
				<mesh filename="package://franka_description/meshes/visual/link4.dae"/>
			</geometry>
		</visual>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 0"/>
			<geometry>
				<cylinder length="0.12" radius="0.09"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 0.06"/>
			<geometry>
				<sphere radius="0.09"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 -0.06"/>
			<geometry>
				<sphere radius="0.09"/>
			</geometry>
		</collision>
	</link>
	<joint name="fr3_joint4" type="revolute">
		<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
		<origin rpy="1.5707963267948966 0 0" xyz="0.0825 0 0"/>
		<parent link="fr3_link3"/>
		<child link="fr3_link4"/>
		<axis xyz="0 0 1"/>
		<limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
	</joint>
	<link name="fr3_link5">
		<visual>
			<geometry>
				<mesh filename="package://franka_description/meshes/visual/link5.dae"/>
			</geometry>
		</visual>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 -0.26"/>
			<geometry>
				<cylinder length="0.1" radius="0.09"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 -0.31"/>
			<geometry>
				<sphere radius="0.09"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 -0.21"/>
			<geometry>
				<sphere radius="0.09"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 0.08 -0.13"/>
			<geometry>
				<cylinder length="0.14" radius="0.055"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 0.08 -0.06"/>
			<geometry>
				<sphere radius="0.055"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 0.08 -0.20"/>
			<geometry>
				<sphere radius="0.055"/>
			</geometry>
		</collision>
	</link>
	<joint name="fr3_joint5" type="revolute">
		<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
		<origin rpy="-1.5707963267948966 0 0" xyz="-0.0825 0.384 0"/>
		<parent link="fr3_link4"/>
		<child link="fr3_link5"/>
		<axis xyz="0 0 1"/>
		<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
	</joint>
	<link name="fr3_link6">
		<visual>
			<geometry>
				<mesh filename="package://franka_description/meshes/visual/link6.dae"/>
			</geometry>
		</visual>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 -0.03"/>
			<geometry>
				<cylinder length="0.08" radius="0.08"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 0.01"/>
			<geometry>
				<sphere radius="0.08"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 -0.07"/>
			<geometry>
				<sphere radius="0.08"/>
			</geometry>
		</collision>
	</link>
	<joint name="fr3_joint6" type="revolute">
		<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
		<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
		<parent link="fr3_link5"/>
		<child link="fr3_link6"/>
		<axis xyz="0 0 1"/>
		<limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
	</joint>
	<link name="fr3_link7">
		<visual>
			<geometry>
				<mesh filename="package://franka_description/meshes/visual/link7.dae"/>
			</geometry>
		</visual>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 0.01"/>
			<geometry>
				<cylinder length="0.14" radius="0.07"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 0.08"/>
			<geometry>
				<sphere radius="0.07"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 -0.06"/>
			<geometry>
				<sphere radius="0.07"/>
			</geometry>
		</collision>
	</link>
	<joint name="fr3_joint7" type="revolute">
		<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
		<origin rpy="1.5707963267948966 0 0" xyz="0.088 0 0"/>
		<parent link="fr3_link6"/>
		<child link="fr3_link7"/>
		<axis xyz="0 0 1"/>
		<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
	</joint>
	<link name="fr3_link8">
		<collision>
			<origin rpy="3.141592653589793 1.5707963267948966 1.5707963267948966" xyz="0.0424 0.0424 -0.0250"/>
			<geometry>
				<cylinder length="0.01" radius="0.06"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0.0424 0.0424 -0.02"/>
			<geometry>
				<sphere radius="0.06"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0.0424 0.0424 -0.03"/>
			<geometry>
				<sphere radius="0.06"/>
			</geometry>
		</collision>
	</link>
	<joint name="fr3_joint8" type="fixed">
		<origin rpy="0 0 0" xyz="0 0 0.107"/>
		<parent link="fr3_link7"/>
		<child link="fr3_link8"/>
	</joint>
	<joint name="fr3_hand_joint" type="fixed">
		<parent link="fr3_link8"/>
		<child link="fr3_hand"/>
		<origin rpy="0 0 -0.7853981633974483" xyz="0 0 0"/>
	</joint>
	<link name="fr3_hand">
		<visual>
			<geometry>
				<mesh filename="package://franka_description/meshes/visual/hand.dae"/>
			</geometry>
		</visual>
		<collision>
			<origin rpy="0 1.5707963267948966 1.5707963267948966" xyz="0 0 0.04"/>
			<geometry>
				<cylinder length="0.1" radius="0.07"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 -0.05 0.04"/>
			<geometry>
				<sphere radius="0.07"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 0.05 0.04"/>
			<geometry>
				<sphere radius="0.07"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 1.5707963267948966 1.5707963267948966" xyz="0 0 0.1"/>
			<geometry>
				<cylinder length="0.1" radius="0.05"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 -0.05 0.1"/>
			<geometry>
				<sphere radius="0.05"/>
			</geometry>
		</collision>
		<collision>
			<origin rpy="0 0 0" xyz="0 0.05 0.1"/>
			<geometry>
				<sphere radius="0.05"/>
			</geometry>
		</collision>
	</link>
	<!-- Define the hand_tcp frame -->
	<link name="fr3_hand_tcp"/>
	<joint name="fr3_hand_tcp_joint" type="fixed">
		<origin rpy="0 0 0" xyz="0 0 0.1034"/>
		<parent link="fr3_hand"/>
		<child link="fr3_hand_tcp"/>
	</joint>
	<link name="fr3_leftfinger">
		<visual>
			<geometry>
				<mesh filename="package://franka_description/meshes/visual/finger.dae"/>
			</geometry>
		</visual>
	</link>
	<link name="fr3_rightfinger">
		<visual>
			<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
			<geometry>
				<mesh filename="package://franka_description/meshes/visual/finger.dae"/>
			</geometry>
		</visual>
	</link>
	<joint name="fr3_finger_joint1" type="prismatic">
		<parent link="fr3_hand"/>
		<child link="fr3_leftfinger"/>
		<origin rpy="0 0 0" xyz="0 0 0.0584"/>
		<axis xyz="0 1 0"/>
		<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
	</joint>
	<joint name="fr3_finger_joint2" type="prismatic">
		<parent link="fr3_hand"/>
		<child link="fr3_rightfinger"/>
		<origin rpy="0 0 0" xyz="0 0 0.0584"/>
		<axis xyz="0 -1 0"/>
		<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
		<mimic joint="fr3_finger_joint1"/>
	</joint>
	<ros2_control name="FrankaHardwareInterface" type="system">
		<hardware>
			<plugin>franka_hardware/FrankaHardwareInterface</plugin>
			<param name="robot_ip">localhost</param>
		</hardware>
		<joint name="fr3_joint1">
			<param name="initial_position">0.0</param>
			<command_interface name="effort"/>
			<command_interface name="velocity"/>
			<command_interface name="position"/>
			<state_interface name="position"/>
			<state_interface name="velocity"/>
			<state_interface name="effort"/>
		</joint>
		<joint name="fr3_joint2">
			<param name="initial_position">-0.7853981633974483</param>
			<command_interface name="effort"/>
			<command_interface name="velocity"/>
			<command_interface name="position"/>
			<state_interface name="position"/>
			<state_interface name="velocity"/>
			<state_interface name="effort"/>
		</joint>
		<joint name="fr3_joint3">
			<param name="initial_position">0.0</param>
			<command_interface name="effort"/>
			<command_interface name="velocity"/>
			<command_interface name="position"/>
			<state_interface name="position"/>
			<state_interface name="velocity"/>
			<state_interface name="effort"/>
		</joint>
		<joint name="fr3_joint4">
			<param name="initial_position">-2.356194490192345</param>
			<command_interface name="effort"/>
			<command_interface name="velocity"/>
			<command_interface name="position"/>
			<state_interface name="position"/>
			<state_interface name="velocity"/>
			<state_interface name="effort"/>
		</joint>
		<joint name="fr3_joint5">
			<param name="initial_position">0.0</param>
			<command_interface name="effort"/>
			<command_interface name="velocity"/>
			<command_interface name="position"/>
			<state_interface name="position"/>
			<state_interface name="velocity"/>
			<state_interface name="effort"/>
		</joint>
		<joint name="fr3_joint6">
			<param name="initial_position">1.5707963267948966</param>
			<command_interface name="effort"/>
			<command_interface name="velocity"/>
			<command_interface name="position"/>
			<state_interface name="position"/>
			<state_interface name="velocity"/>
			<state_interface name="effort"/>
		</joint>
		<joint name="fr3_joint7">
			<param name="initial_position">0.7853981633974483</param>
			<command_interface name="effort"/>
			<command_interface name="velocity"/>
			<command_interface name="position"/>
			<state_interface name="position"/>
			<state_interface name="velocity"/>
			<state_interface name="effort"/>
		</joint>
	</ros2_control>
</robot>