Skip to content

Commit

Permalink
added calibration data for cob3-1
Browse files Browse the repository at this point in the history
  • Loading branch information
Sven Schneider committed Jul 19, 2012
1 parent cbd34e6 commit 536cb38
Show file tree
Hide file tree
Showing 5 changed files with 207 additions and 36 deletions.
73 changes: 37 additions & 36 deletions cob3-1/calibration/calibration.urdf.xacro
Expand Up @@ -39,40 +39,41 @@
<property name="tray_yaw" value="0"/>

<!-- torso mount positions | relative to base_link -->
<property name="torso_x" value="0.1523087993"/>
<property name="torso_y" value="-0.0441619833"/>
<property name="torso_z" value="0.8846128253"/>
<property name="torso_roll" value="1.5977820225"/>
<property name="torso_pitch" value="-0.0605637991"/>
<property name="torso_yaw" value="0.0359995364"/>
<property name="torso_x" value="0.155"/>
<property name="torso_y" value="0.0"/>
<property name="torso_z" value="0.892"/>
<property name="torso_roll" value="3.1415926"/>
<property name="torso_pitch" value="0.0"/>
<property name="torso_yaw" value="0.0"/>

<!-- arm mount positions | relative to base_link -->
<property name="arm_x" value="-0.0146879612"/>
<property name="arm_y" value="-0.0862146632"/>
<property name="arm_z" value="0.8778855633"/>
<property name="arm_roll" value="0.8233561573"/>
<property name="arm_pitch" value="-0.0006376545"/>
<property name="arm_yaw" value="-0.2866555181"/>
<property name="arm_x" value="-0.015982"/>
<property name="arm_y" value="-0.080992"/>
<property name="arm_z" value="0.875995"/>
<property name="arm_roll" value="0.7854"/>
<property name="arm_pitch" value="0.0"/>
<property name="arm_yaw" value="-0.2618"/>

<!-- *********************************** -->
<!-- kinematic chain reference positions -->
<!-- *********************************** -->
<!-- torso chain calibration_offest! | relative to reference position (see below)-->
<!-- not used right now
<property name="torso_lower_neck_pan_cal_offset" value="0.0"/>
<property name="torso_lower_neck_tilt_cal_offset" value="0.0"/>
<property name="torso_pan_cal_offset" value="0.0"/>
<property name="torso_upper_neck_pan_cal_offset" value="0.0"/>
<property name="torso_upper_neck_tilt_cal_offset" value="0.0"/>
-->

<!-- torso reference positions | used as calibration_rising for torso chain -->
<!-- ! set these values to make torso straight with (0, 0, 0) joint angles ! -->
<property name="torso_lower_neck_pan_ref" value="-0.5470" />
<property name="torso_lower_neck_tilt_ref" value="0.0344" />
<property name="torso_upper_neck_pan_ref" value="0.0247" />
<property name="torso_upper_neck_tilt_ref" value="0.02201" />
<property name="torso_lower_neck_pan_ref" value="-0.5306" />
<property name="torso_lower_neck_tilt_ref" value="0.0320" />
<property name="torso_upper_neck_pan_ref" value="0.5481" />
<property name="torso_upper_neck_tilt_ref" value="0.0229" />

<!-- tray reference position | used as calibration_rising for tray chain -->
<property name="tray_ref" value="-0.2"/>
<property name="tray_ref" value="2.5041" />

<!-- head camera axis reference position | used as calibration_rising for head axis chain -->
<property name="head_axis_ref" value="-0.1"/>
Expand All @@ -90,26 +91,26 @@
<!-- camera calibration properties -->
<!-- ***************************** -->
<!-- left | handeye calibration | relative to head_axis_link -->
<property name="cam_l_x" value="-0.0171250371"/>
<property name="cam_l_y" value="-0.0389120491"/>
<property name="cam_l_z" value="-0.12027305"/>
<property name="cam_l_roll" value="2.149627771"/>
<property name="cam_l_pitch" value="-1.5608998734"/>
<property name="cam_l_yaw" value="-2.2074792588"/>
<property name="cam_l_x" value="-0.12" />
<property name="cam_l_y" value="0.01" />
<property name="cam_l_z" value="0.02" />
<property name="cam_l_roll" value="0.0" />
<property name="cam_l_pitch" value="0.0" />
<property name="cam_l_yaw" value="0.0" />

<!-- right | stereo baseline calibration | relative to left camera -->
<property name="cam_r_x" value="0.121342166993"/>
<property name="cam_r_y" value="0.000964077625598"/>
<property name="cam_r_z" value="-0.00119322781437"/>
<property name="cam_r_roll" value="0.004726030387"/>
<property name="cam_r_pitch" value="0.0169849756686"/>
<property name="cam_r_yaw" value="0.0118232483039"/>
<property name="cam_r_x" value="-0.00075932" />
<property name="cam_r_y" value="-0.00006386" />
<property name="cam_r_z" value="-0.00026527" />
<property name="cam_r_roll" value="-0.00006" />
<property name="cam_r_pitch" value="0.00546" />
<property name="cam_r_yaw" value="0.00676" />

<!-- cam3d | handeye calibration | relative to head_axis_link -->
<property name="cam3d_x" value="-0.0392332921"/>
<property name="cam3d_y" value="-0.0725275279"/>
<property name="cam3d_z" value="-0.041924241"/>
<property name="cam3d_roll" value="0.0974172003"/>
<property name="cam3d_pitch" value="-1.5456482961"/>
<property name="cam3d_yaw" value="-0.1128148583"/>
<property name="cam3d_x" value="0.0" />
<property name="cam3d_y" value="0.0" />
<property name="cam3d_z" value="0.0" />
<property name="cam3d_roll" value="0.0" />
<property name="cam3d_pitch" value="0.0" />
<property name="cam3d_yaw" value="0.0" />
</robot>
20 changes: 20 additions & 0 deletions cob3-1/calibration/cameras/kinect_rgb.yaml
@@ -0,0 +1,20 @@
image_width: 640
image_height: 480
camera_name: right
camera_matrix:
rows: 3
cols: 3
data: [526.11430030557256, 0, 300.54291573150408, 0, 524.98747064798692, 257.39758836454314, 0, 0, 1]
distortion_coefficients:
rows: 1
cols: 5
data: [0.24327860987087502, -0.82075692895965324, -0.00035209329984236648, -0.0019559583281848580, 0.95382058300602524]
rectification_matrix:
rows: 3
cols: 3
data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
rows: 3
cols: 4
data: [526.11430030557256, 0, 300.54291573150408, 0, 0, 524.98747064798692, 257.39758836454314, 0, 0, 0, 1, 0]

21 changes: 21 additions & 0 deletions cob3-1/calibration/cameras/left.yaml
@@ -0,0 +1,21 @@
camera_name: left
frame_id: /head_color_camera_l_link
image_width: 1360
image_height: 1024
camera_matrix:
rows: 3
cols: 3
data: [1451.2239242707201, 0.0, 647.33159357949535, 0.0, 1451.512357466929, 505.19364095186694, 0.0, 0.0, 1.0]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [-0.25648209827532603, 0.21535276176875215, -0.00076700926780839441, -0.0010578565343022676, -0.053446645001753126]
rectification_matrix:
rows: 3
cols: 3
data: [0.99992009750099486, 0.0079444814385283887, -0.0098327935132465714, -0.0079216726602033809, 0.99996584655826892, 0.002356442078415095, 0.0098511783998591895, -0.0022783616212569972, 0.99994888036962026]
projection_matrix:
rows: 3
cols: 4
data: [1455.1678151992817, 0.0, 663.18903350830078, 0.0, 0.0, 1455.1678151992817, 509.86345291137695, 0.0, 0.0, 0.0, 1.0, 0.0]
21 changes: 21 additions & 0 deletions cob3-1/calibration/cameras/right.yaml
@@ -0,0 +1,21 @@
camera_name: right
frame_id: /head_color_camera_l_link
image_width: 1360
image_height: 1024
camera_matrix:
rows: 3
cols: 3
data: [1451.4054199463656, 0.0, 680.64779530745341, 0.0, 1448.1606054066501, 522.62508312410819, 0.0, 0.0, 1.0]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [-0.26540693788495234, 0.25960235060163006, -0.00076220057156406126, -0.0015825195465792492, -0.0082437187321492307]
rectification_matrix:
rows: 3
cols: 3
data: [0.99996690524286469, -0.0038442590186795254, 0.007170083096109235, 0.0038608651288062818, 0.99998989346341471, -0.0023036257259529034, -0.00716115489742946, 0.0023312322118153451, 0.99997164120634441]
projection_matrix:
rows: 3
cols: 4
data: [1455.1678151992817, 0.0, 663.18903350830078, -176.58732580273212, 0.0, 1455.1678151992817, 509.86345291137695, 0.0, 0.0, 0.0, 1.0, 0.0]
108 changes: 108 additions & 0 deletions cob3-1/calibration/default/calibration.urdf.xacro
@@ -0,0 +1,108 @@
<?xml version="1.0"?>
<robot>
<!-- ***************** -->
<!-- base calibration -->
<!-- ***************** -->
<!-- laser front mount positions | relative to base_link -->
<property name="laser_front_x" value="0.325"/>
<property name="laser_front_y" value="0.0"/>
<property name="laser_front_z" value="0.135"/>
<property name="laser_front_roll" value="0"/>
<property name="laser_front_pitch" value="0"/>
<property name="laser_front_yaw" value="0"/>

<!-- laser rear mount positions | relative to base_link -->
<property name="laser_rear_x" value="-0.325"/>
<property name="laser_rear_y" value="0.0"/>
<property name="laser_rear_z" value="0.135"/>
<property name="laser_rear_roll" value="0"/>
<property name="laser_rear_pitch" value="0"/>
<property name="laser_rear_yaw" value="3.1415926"/>

<!-- laser top mount positions | relative to base_link -->
<property name="laser_top_x" value="-0.252"/>
<property name="laser_top_y" value="0.0"/>
<property name="laser_top_z" value="0.42"/>
<property name="laser_top_roll" value="0"/>
<property name="laser_top_pitch" value="0"/>
<property name="laser_top_yaw" value="0"/>

<!-- ***************** -->
<!-- robot calibration -->
<!-- ***************** -->
<!-- torso mount positions | relative to base_link -->
<property name="torso_x" value="0.155"/>
<property name="torso_y" value="0.0"/>
<property name="torso_z" value="0.892"/>
<property name="torso_roll" value="3.1415926"/>
<property name="torso_pitch" value="0.0"/>
<property name="torso_yaw" value="0.0"/>

<!-- arm mount positions | relative to base_link -->
<property name="arm_x" value="-0.015982"/>
<property name="arm_y" value="-0.080992"/>
<property name="arm_z" value="0.875995"/>
<property name="arm_roll" value="0.7854"/>
<property name="arm_pitch" value="0.0"/>
<property name="arm_yaw" value="-0.2618"/>

<!-- *********************************** -->
<!-- kinematic chain reference positions -->
<!-- *********************************** -->
<!-- torso chain calibration_offest! | relative to reference position (see below)-->
<!-- not used right now
<property name="torso_lower_neck_pan_cal_offset" value="0.0"/>
<property name="torso_lower_neck_tilt_cal_offset" value="0.0"/>
<property name="torso_upper_neck_pan_cal_offset" value="0.0"/>
<property name="torso_upper_neck_tilt_cal_offset" value="0.0"/>
-->

<!-- torso reference positions | used as calibration_rising for torso chain -->
<!-- ! set these values to make torso straight with (0, 0, 0) joint angles ! -->
<property name="torso_lower_neck_pan_ref" value="-0.5306" />
<property name="torso_lower_neck_tilt_ref" value="0.0320" />
<property name="torso_upper_neck_pan_ref" value="0.5481" />
<property name="torso_upper_neck_tilt_ref" value="0.0229" />

<!-- tray reference position | used as calibration_rising for tray chain -->
<property name="torso_tray_ref" value="2.5041" />

<!-- head camera axis reference position | used as calibration_rising for head axis chain -->
<property name="head_axis_ref" value="-3.266798" />

<!-- arm reference positions | used as calibration_rising for arm chain -->
<property name="arm_1_ref" value="0.0" />
<property name="arm_2_ref" value="0.0" />
<property name="arm_3_ref" value="0.0" />
<property name="arm_4_ref" value="0.0" />
<property name="arm_5_ref" value="0.0" />
<property name="arm_6_ref" value="0.0" />
<property name="arm_7_ref" value="0.0" />

<!-- ***************************** -->
<!-- camera calibration properties -->
<!-- ***************************** -->
<!-- left | handeye calibration | relative to head_axis_link -->
<property name="cam_l_x" value="-0.12" />
<property name="cam_l_y" value="0.01" />
<property name="cam_l_z" value="0.02" />
<property name="cam_l_roll" value="0.0" />
<property name="cam_l_pitch" value="0.0" />
<property name="cam_l_yaw" value="0.0" />

<!-- right | stereo baseline calibration | relative to left camera -->
<property name="cam_r_x" value="-0.00075932" />
<property name="cam_r_y" value="-0.00006386" />
<property name="cam_r_z" value="-0.00026527" />
<property name="cam_r_roll" value="-0.00006" />
<property name="cam_r_pitch" value="0.00546" />
<property name="cam_r_yaw" value="0.00676" />

<!-- cam3d | handeye calibration | relative to head_axis_link -->
<property name="cam3d_x" value="0.0" />
<property name="cam3d_y" value="0.0" />
<property name="cam3d_z" value="0.0" />
<property name="cam3d_roll" value="0.0" />
<property name="cam3d_pitch" value="0.0" />
<property name="cam3d_yaw" value="0.0" />
</robot>

0 comments on commit 536cb38

Please sign in to comment.