[WIP] Add baxter_calibration

jsk_calibration/baxter_calibration/capture_data/all_pipelines.launch

@@ -0,0 +1,20 @@
+<launch>
+  <include file="$(find jsk_calibration)/baxter_calibration/capture_data/settler.launch" />
+
+  <arg name="use_kinect" default="false" />
+  <group if="$(arg use_kinect)" >
+    <include file="$(find calibration_launch)/capture_data/kinect_pipeline.launch" ns="/camera_remote/rgb">
+      <arg name="image_topic" value="image_rect_color"/> <!-- openni2.launch publishes image_raw -->
+      <arg name="depth_topic" value="/camera/depth/image"/> <!-- this is floar value -->
+      <arg name="camera_info_topic" value="camera_info"/>
+    </include>
+  </group>
+  <group unless="$(arg use_kinect)" >
+    <include file="$(find calibration_launch)/capture_data/monocam_pipeline.launch"
+             ns="/right_hand_camera/rgb">
+      <arg name="image_topic" value="image_rect_color"/> <!-- this should be image proc -->
+      <arg name="frequency" value="5" />
+    </include>
+  </group>
+  <include file="$(find jsk_calibration)/baxter_calibration/capture_data/interval.launch" />
+</launch>

jsk_calibration/baxter_calibration/capture_data/all_viewers.launch

@@ -0,0 +1,16 @@
+
+<launch>
+
+  <!-- Hack to create the directory -->
+  <param name="mkdir_tmp" command="mkdir -m 777 -p /tmp/baxter_calibration" />
+  <param name="mkdir_tmp" command="mkdir -m 777 -p /tmp/baxter_calibration/cb_fail" />
+
+  <param name="/right_hand_camera/rgb/annotated_viewer/filename_format" type="string"
+         value="/tmp/baxter_calibration/cb_fail/cb_monocam_%04i.jpg" />
+
+  <include file="$(find calibration_launch)/capture_data/annotated_viewer.launch"
+           ns="/right_hand_camera/rgb" >
+    <arg name="image_topic" value="image_rect_annotated" />
+  </include>
+
+</launch>

jsk_calibration/baxter_calibration/capture_data/capture_data.launch

@@ -0,0 +1,11 @@
+<launch>
+  <node pkg="jsk_calibration" type="joint_states_appender.py" name="joint_states_appender">
+    <remap from="joint_states" to="/robot/joint_states" />
+  </node>
+  <group>
+    <remap from="/joint_states" to="/joint_states_appended" />
+    <include file="$(find jsk_calibration)/baxter_calibration/capture_data/all_viewers.launch"/>
+    <include file="$(find jsk_calibration)/baxter_calibration/capture_data/all_pipelines.launch"/>
+    <include file="$(find jsk_calibration)/baxter_calibration/capture_data/capture_exec.launch"/>
+  </group>
+</launch>

jsk_calibration/baxter_calibration/capture_data/capture_exec.launch

@@ -0,0 +1,21 @@
+<launch>
+  <node type="capture_exec.py"
+        pkg="calibration_launch"
+        name="calibration_exec"
+        args="$(find jsk_calibration)/baxter_calibration/capture_data/samples/ $(find jsk_calibration)/baxter_calibration/capture_data/hardware_config $(find jsk_calibration)/baxter_calibration/estimate_params/config/system.yaml"
+        output="screen" >
+    <remap from="head_camera/camera_info" to="/right_hand_camera/rgb/camera_info"/>
+    <remap from="head_camera/image_rect"  to="/right_hand_camera/rgb/image_rect_throttle"/>
+    <remap from="head_camera/image"       to="/right_hand_camera/rgb/image_rect_throttle"/>
+    <remap from="head_camera/features"    to="/right_hand_camera/rgb/features"/>
+  </node>
+
+  <node type="urdf_pub.py" pkg="calibration_launch" name="urdf_pub"/>
+
+  <node type="record" pkg="rosbag" name="robot_measurement_recorder" output="screen"
+        args="-O /tmp/baxter_calibration/cal_measurements robot_measurement robot_description" >
+    <!-- Hack to create the directory -->
+    <param name="mkdir_tmp" command="mkdir -m 777 -p /tmp/baxter_calibration" />
+  </node>
+
+</launch>

jsk_calibration/baxter_calibration/capture_data/hardware_config/cam_config.yaml

@@ -0,0 +1,21 @@
+head_camera:
+  cb_detector_config: /right_hand_camera/rgb/cb_detector_config
+  led_detector_config: /right_hand_camera/rgb/led_detector
+  settler_config: /right_hand_camera/rgb/monocam_settler_config
+  configs:
+    mmurooka_board:
+      settler:
+        tolerance: 2.00
+        ignore_failures: True
+        max_step: 2.0 
+        cache_size: 20
+      cb_detector:
+        active: True
+        num_x: 6
+        num_y: 5
+        width_scaling: 1
+        height_scaling: 1
+        subpixel_window: 4
+        subpixel_zero_zone: 1
+      led_detector:
+        active: False

jsk_calibration/baxter_calibration/capture_data/hardware_config/chain_config.yaml

@@ -0,0 +1,59 @@
+LARM_chain:
+  settler_config: /LARM_chain/settler_config
+  configs:
+    tight_tol:
+      settler:
+        joint_names:
+          - left_s0
+          - left_s1
+          - left_e0
+          - left_e1
+          - left_w0
+          - left_w1
+          - left_w2
+        tolerances:
+          - 0.002
+          - 0.002
+          - 0.002
+          - 0.002
+          - 0.002
+          - 0.002
+          - 0.002
+        max_step: 1.0
+        cache_size: 1500
+RARM_chain:
+  settler_config: /RARM_chain/settler_config
+  configs:
+    tight_tol:
+      settler:
+        joint_names:
+          - right_s0
+          - right_s1
+          - right_e0
+          - right_e1
+          - right_w0
+          - right_w1
+          - right_w2
+        tolerances:
+          - 0.002
+          - 0.002
+          - 0.002
+          - 0.002
+          - 0.002
+          - 0.002
+          - 0.002
+        max_step: 1.0
+        cache_size: 1500
+head_chain:
+  settler_config: /head_chain/settler_config
+  configs:
+    tight_tol:
+      settler:
+        joint_names:
+          - head_nod
+          - head_pan
+        tolerances:
+          - 0.002
+          - 0.002
+        max_step: 1.0
+        cache_size: 1500

jsk_calibration/baxter_calibration/capture_data/hardware_config/controller_config.yaml

@@ -0,0 +1,25 @@
+LARM_controller:
+  topic: /robot/limb/left/command_joint_position
+  joint_names:
+    - left_s0
+    - left_s1
+    - left_e0
+    - left_e1
+    - left_w0
+    - left_w1
+    - left_w2
+RARM_controller:
+  topic: /robot/limb/right/command_joint_position
+  joint_names:
+    - right_s0
+    - right_s1
+    - right_e0
+    - right_e1
+    - right_w0
+    - right_w1
+    - right_w2
+head_controller:
+  topic: /robot/head/head_pan
+  joint_names:
+    - head_nod
+    - head_pan

jsk_calibration/baxter_calibration/capture_data/hardware_config/laser_config.yaml

@@ -0,0 +1 @@
+# laser config

jsk_calibration/baxter_calibration/capture_data/interval.launch

@@ -0,0 +1,11 @@
+<launch>
+  <node type="interval_intersection_action"
+        pkg="interval_intersection"
+        name="interval_intersection"
+        output="screen">
+    <remap from="LARM_chain" to="LARM_chain/settled_interval" />
+    <remap from="RARM_chain" to="RARM_chain/settled_interval" />
+    <remap from="head_chain" to="head_chain/settled_interval" />
+    <remap from="head_camera" to="/right_hand_camera/rgb/settled_interval" />
+  </node>
+</launch>

jsk_calibration/baxter_calibration/capture_data/samples/00_RARM/RARM_0000.yaml

@@ -0,0 +1,20 @@
+camera_measurements:
+- {cam_id: head_camera, config: mmurooka_board}
+joint_commands:
+- controller: LARM_controller
+  segments:
+  - duration: 15.0
+    positions: [0.9514522790906927, -0.27304803482825285, -1.5163403088034215, 2.1816964715779514, 0.35818519574053626, -0.3420775520738806, 1.0408061554757946]
+- controller: RARM_controller
+  segments:
+  - duration: 15.0
+    positions: [-0.6319994301104147, 0.46556308731098345, 2.0378987945061393, 1.446926819122355, -0.08398541813889233, 2.09312101203924, -1.10369909507141]
+- controller: head_controller
+  segments:
+  - duration: 15.0
+    positions: [-0.8697673794388542]
+joint_measurements:
+- {chain_id: RARM_chain,  config: tight_tol}
+- {chain_id: head_chain,  config: tight_tol}
+sample_id: RARM_0000
+target: {chain_id: RARM_chain, target_id: mmurooka_board}

jsk_calibration/baxter_calibration/capture_data/samples/00_RARM/RARM_0001.yaml

@@ -0,0 +1,20 @@
+camera_measurements:
+- {cam_id: head_camera, config: mmurooka_board}
+joint_commands:
+- controller: LARM_controller
+  segments:
+  - duration: 15.0
+    positions: [0.9514522790906927, -0.27304803482825285, -1.5163403088034215, 2.1816964715779514, 0.35818519574053626, -0.3420775520738806, 1.0408061554757946]
+- controller: RARM_controller
+  segments:
+  - duration: 15.0
+    positions: [-0.660379, 0.526922, 2.03137, 1.51864, -0.245437, 2.08506, -1.04924]
+- controller: head_controller
+  segments:
+  - duration: 15.0
+    positions: [-0.8697673794388542]
+joint_measurements:
+- {chain_id: RARM_chain,  config: tight_tol}
+- {chain_id: head_chain,  config: tight_tol}
+sample_id: RARM_0001
+target: {chain_id: RARM_chain, target_id: mmurooka_board}

jsk_calibration/baxter_calibration/capture_data/samples/00_RARM/RARM_0002.yaml

@@ -0,0 +1,20 @@
+camera_measurements:
+- {cam_id: head_camera, config: mmurooka_board}
+joint_commands:
+- controller: LARM_controller
+  segments:
+  - duration: 15.0
+    positions: [0.9514522790906927, -0.27304803482825285, -1.5163403088034215, 2.1816964715779514, 0.35818519574053626, -0.3420775520738806, 1.0408061554757946]
+- controller: RARM_controller
+  segments:
+  - duration: 15.0
+    positions: [-0.428364, 0.582529, 2.13032, 1.64826, -0.198267, 2.05208, -1.01626]
+- controller: head_controller
+  segments:
+  - duration: 15.0
+    positions: [-0.8697673794388542]
+joint_measurements:
+- {chain_id: RARM_chain,  config: tight_tol}
+- {chain_id: head_chain,  config: tight_tol}
+sample_id: RARM_0002
+target: {chain_id: RARM_chain, target_id: mmurooka_board}

jsk_calibration/baxter_calibration/capture_data/samples/00_RARM/RARM_0003.yaml

@@ -0,0 +1,20 @@
+camera_measurements:
+- {cam_id: head_camera, config: mmurooka_board}
+joint_commands:
+- controller: LARM_controller
+  segments:
+  - duration: 15.0
+    positions: [0.9514522790906927, -0.27304803482825285, -1.5163403088034215, 2.1816964715779514, 0.35818519574053626, -0.3420775520738806, 1.0408061554757946]
+- controller: RARM_controller
+  segments:
+  - duration: 15.0
+    positions: [-0.492791, 0.635068, 2.24728, 1.68239, -0.251956, 2.00453, -0.697194]
+- controller: head_controller
+  segments:
+  - duration: 15.0
+    positions: [-0.8697673794388542]
+joint_measurements:
+- {chain_id: RARM_chain,  config: tight_tol}
+- {chain_id: head_chain,  config: tight_tol}
+sample_id: RARM_0003
+target: {chain_id: RARM_chain, target_id: mmurooka_board}

jsk_calibration/baxter_calibration/capture_data/samples/00_RARM/RARM_0004.yaml

@@ -0,0 +1,20 @@
+camera_measurements:
+- {cam_id: head_camera, config: mmurooka_board}
+joint_commands:
+- controller: LARM_controller
+  segments:
+  - duration: 15.0
+    positions: [0.9514522790906927, -0.27304803482825285, -1.5163403088034215, 2.1816964715779514, 0.35818519574053626, -0.3420775520738806, 1.0408061554757946]
+- controller: RARM_controller
+  segments:
+  - duration: 15.0
+    positions: [-0.533058, 0.520786, 2.22887, 1.75641, -0.060209, 1.98919, -0.445621]
+- controller: head_controller
+  segments:
+  - duration: 15.0
+    positions: [-0.8697673794388542]
+joint_measurements:
+- {chain_id: RARM_chain,  config: tight_tol}
+- {chain_id: head_chain,  config: tight_tol}
+sample_id: RARM_0004
+target: {chain_id: RARM_chain, target_id: mmurooka_board}

jsk_calibration/baxter_calibration/capture_data/samples/00_RARM/RARM_0005.yaml

@@ -0,0 +1,20 @@
+camera_measurements:
+- {cam_id: head_camera, config: mmurooka_board}
+joint_commands:
+- controller: LARM_controller
+  segments:
+  - duration: 15.0
+    positions: [0.9514522790906927, -0.27304803482825285, -1.5163403088034215, 2.1816964715779514, 0.35818519574053626, -0.3420775520738806, 1.0408061554757946]
+- controller: RARM_controller
+  segments:
+  - duration: 15.0
+    positions: [-0.740913, 0.509282, 2.26032, 1.84231, 0.029529, 1.93703, -0.4345]
+- controller: head_controller
+  segments:
+  - duration: 15.0
+    positions: [-0.8697673794388542]
+joint_measurements:
+- {chain_id: RARM_chain,  config: tight_tol}
+- {chain_id: head_chain,  config: tight_tol}
+sample_id: RARM_0005
+target: {chain_id: RARM_chain, target_id: mmurooka_board}

jsk_calibration/baxter_calibration/capture_data/samples/00_RARM/RARM_0006.yaml

@@ -0,0 +1,20 @@
+camera_measurements:
+- {cam_id: head_camera, config: mmurooka_board}
+joint_commands:
+- controller: LARM_controller
+  segments:
+  - duration: 15.0
+    positions: [0.9514522790906927, -0.27304803482825285, -1.5163403088034215, 2.1816964715779514, 0.35818519574053626, -0.3420775520738806, 1.0408061554757946]
+- controller: RARM_controller
+  segments:
+  - duration: 15.0
+    positions: [-0.709466, 0.477068, 2.25994, 1.86532, 0.075549, 1.91863, -0.723272]
+- controller: head_controller
+  segments:
+  - duration: 15.0
+    positions: [-0.8697673794388542]
+joint_measurements:
+- {chain_id: RARM_chain,  config: tight_tol}
+- {chain_id: head_chain,  config: tight_tol}
+sample_id: RARM_0006
+target: {chain_id: RARM_chain, target_id: mmurooka_board}

jsk_calibration/baxter_calibration/capture_data/samples/00_RARM/RARM_0007.yaml

@@ -0,0 +1,20 @@
+camera_measurements:
+- {cam_id: head_camera, config: mmurooka_board}
+joint_commands:
+- controller: LARM_controller
+  segments:
+  - duration: 15.0
+    positions: [0.9514522790906927, -0.27304803482825285, -1.5163403088034215, 2.1816964715779514, 0.35818519574053626, -0.3420775520738806, 1.0408061554757946]
+- controller: RARM_controller
+  segments:
+  - duration: 15.0
+    positions: [-0.643121, 0.469782, 2.27758, 1.84921, -0.021476, 1.88028, -1.01741]
+- controller: head_controller
+  segments:
+  - duration: 15.0
+    positions: [-0.8697673794388542]
+joint_measurements:
+- {chain_id: RARM_chain,  config: tight_tol}
+- {chain_id: head_chain,  config: tight_tol}
+sample_id: RARM_0007
+target: {chain_id: RARM_chain, target_id: mmurooka_board}

jsk_calibration/baxter_calibration/capture_data/samples/00_RARM/RARM_0008.yaml

@@ -0,0 +1,20 @@
+camera_measurements:
+- {cam_id: head_camera, config: mmurooka_board}
+joint_commands:
+- controller: LARM_controller
+  segments:
+  - duration: 15.0
+    positions: [0.9514522790906927, -0.27304803482825285, -1.5163403088034215, 2.1816964715779514, 0.35818519574053626, -0.3420775520738806, 1.0408061554757946]
+- controller: RARM_controller
+  segments:
+  - duration: 15.0
+    positions: [-0.267296, 0.454058, 2.72013, 1.71116, 0.16567, 2.094, -0.228947]
+- controller: head_controller
+  segments:
+  - duration: 15.0
+    positions: [-0.8697673794388542]
+joint_measurements:
+- {chain_id: RARM_chain,  config: tight_tol}
+- {chain_id: head_chain,  config: tight_tol}
+sample_id: RARM_0008
+target: {chain_id: RARM_chain, target_id: mmurooka_board}