<!--Frames.xml-->
<Data>
<Frames> 
<!--Base of kinematic chain-->
<Frame name="fob_base" base="" a="0" b="0" g="0" x="0" y="0" z="0" />

<!--helper frame for calibration. This frame is identical as root, but has margin for calibration-->
<Frame name="ptu_base_offset" base="fob_base" a="0" b="0" g="0" x="0" y="0" z="0" min_x="-1000" max_x="1000" min_y="-1000" max_y="1000" min_z="-1000" max_z="1000"/> 

<!--real TF from FoBBase (with calibration offset) to PTU-->
<Frame name="ptu_base" base="ptu_base_offset" a="0" b="0" g="0" x="923.776+683" y="332+873" z="158.5-549.5-95.0"/> 
<Frame name="ptu" base="ptu_base" a="0" b="0" g="-45" y="0" x=" 0" z="0 " />

<!--helper frame for PTU pan input (Variable and data=true)-->
<Frame name="ptu_pan" base="ptu" a="0" b="0" g="-([1])" x="0" y="0" z="0" data="true" />

<!--helper frame for PTU tilt input (Variable and data=true)-->
<Frame name="ptu_tilt" base="ptu_pan" a="([2])" b="0" g="0" x="0" y="0" z="0" data="true"/>

<!--TF from PTU to camera-->
<Frame name="camera_left_frame" base="ptu_tilt" a="90" b="0" g="0" x="-70" y="-37" z="-128+95" />

<!--Datapoint of checkerboard in camera frame (Variables and data=true)-->
<Frame name="checkerboard_camera" base="camera_left_frame" x="([10])*1000" y="([11])*1000" z="([12])*1000" data="true" />

<!--Datapoint of fob tracker in fob_base frame (Variables and data=true)-->
<Frame name="tracker_left_raw" base="fob_base" x="([3])*1000" y="([4])*1000" z="([5])*1000" qw="([6])" qx="([7])" qy="([8])" qz="([9])" data="true" />

<!--Offset from tracker to checkerboard point-->
<Frame name="checkerboard_tracker" base="tracker_left_raw" a="0" b="0" g="0" x="0" y="0" z="20" /> 
</Frames>
	
<Goal>
<!--Match both checkerboard points (they are the same points in real)-->
<Position first="checkerboard_camera" second="checkerboard_tracker"/>
<Orientation first="checkerboard_camera" second="checkerboard_tracker"/>
</Goal>
</Data>