[WIP] Add baxter_calibration

[wkentaro]

Requirements

• https://github.com/wkentaro/baxter_interface/tree/baxter_calibration
• https://github.com/start-jsk/jsk_apc/tree/baxter_calibration

Usage

roslaunch jsk_arc2017_baxter baxter.launch
rosrun jsk_perception image_publisher.py _file_name:=`pwd`/6x5_cb.png
rosrun jsk_baxter_startup xdisplay_image_topic.py /image_publisher/output

rosrun baxter_interface joint_trajectory_command_server.py

roscd jsk_calibration/baxter_calibration/capture_data
roslaunch ./capture_data.launch

cc @YutoUchimi

[wkentaro]

@YoheiKakiuchi I tried to use this program, and got error Didn't get good data, and I found this is because all yeild_rates in calibration_msgs/IntervalStatus is all 0.
Have you ever encountered this problem?

% rosmsg show calibration_msgs/IntervalStatus
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string[] names
float32[] yeild_rates

[wkentaro]

I had error below, so maybe Joint origin is rotated, calibration will fail is fatal?

Starting The Multi Step [Covariance] Estimator Node

Using Bagfile: /home/wkentaro/baxter_calibration_right_hand_camera/cal_measurements.bag

The following error sensors have been loaded:

  rectified_cams sensors
   - head_camera

  chains sensors
   - LARM_chain
   - RARM_chain
   - head_chain

Loaded the calibration steps to execute in the following order:
 - baxter - 00 - Estimating Checkerboard Locations
 - baxter - 01 - Adding Camera Locations
 - baxter - 02a - Adding Camera Locations
Joint origin is rotated, calibration will fail:  right_s1
Joint origin is rotated, calibration will fail:  right_e0
Joint origin is rotated, calibration will fail:  right_e1
Joint origin is rotated, calibration will fail:  right_w0
Joint origin is rotated, calibration will fail:  right_w1
Joint origin is rotated, calibration will fail:  right_w2
Joint origin is rotated, calibration will fail:  left_s1
Joint origin is rotated, calibration will fail:  left_e0
Joint origin is rotated, calibration will fail:  left_e1
Joint origin is rotated, calibration will fail:  left_w0
Joint origin is rotated, calibration will fail:  left_w1
Joint origin is rotated, calibration will fail:  left_w2

******************************
Beginning [baxter - 00 - Estimating Checkerboard Locations]
[tilting_lasers] section doesn't exist. Skipping
[tilting_lasers] section doesn't exist. Skipping
[tilting_lasers] section doesn't exist. Skipping
[tilting_lasers] section doesn't exist. Skipping
[tilting_lasers] section doesn't exist. Skipping
[tilting_lasers] section doesn't exist. Skipping
[tilting_lasers] section doesn't exist. Skipping
[tilting_lasers] section doesn't exist. Skipping
[tilting_lasers] section doesn't exist. Skipping
[tilting_lasers] section doesn't exist. Skipping
[tilting_lasers] section doesn't exist. Skipping
[tilting_lasers] section doesn't exist. Skipping
[tilting_lasers] section doesn't exist. Skipping
[tilting_lasers] section doesn't exist. Skipping
Executing step with the following Sensors:
  chains Sensors:
   - LARM_chain (0)
   - RARM_chain (14)

  rectified_cams Sensors:
   - head_camera (14)

Sensor breakdown (By Sample):
  0) RARM_chain, head_camera
  1) RARM_chain, head_camera
  2) RARM_chain, head_camera
  3) RARM_chain, head_camera
  4) RARM_chain, head_camera
  5) RARM_chain, head_camera
  6) RARM_chain, head_camera
  7) RARM_chain, head_camera
  8) RARM_chain, head_camera
  9) RARM_chain, head_camera
  10) RARM_chain, head_camera
  11) RARM_chain, head_camera
  12) RARM_chain, head_camera
  13) RARM_chain, head_camera
Pose Guesses:
[[  6.21389020e-01   2.51915500e-02   8.04876700e-01  -5.17664070e-01
   -1.02213520e+00   4.30054000e-01]
 [  5.31313830e-01   1.85726980e-01   7.97788400e-01  -1.05697388e+00
   -1.22941420e+00   1.29240951e+00]
 [  5.31242820e-01   1.85725820e-01   7.97808790e-01  -7.59552000e-02
   -1.07722330e+00   3.60156390e-01]
 [  5.31312870e-01   1.85727850e-01   7.97788010e-01   1.52583429e+00
   -1.14768884e+00  -8.38106500e-01]
 [  5.31312470e-01   1.85727660e-01   7.97787960e-01   1.04438406e+00
   -9.83267300e-01  -9.37675960e-01]
 [  5.30668090e-01   1.85125630e-01   7.97717640e-01   7.43839110e-01
   -7.82723420e-01  -5.51149500e-01]
 [  5.88184420e-01   2.03352860e-01   8.04834640e-01  -5.40914960e-01
   -9.16223400e-01   1.01678360e+00]
 [  5.88244600e-01   2.03216360e-01   8.04803320e-01   1.18645824e+00
   -1.89096102e+00  -9.06666850e-01]
 [  5.88310640e-01   2.03222470e-01   8.04857040e-01   2.00695344e+00
   -5.55434630e-01  -1.34719380e+00]
 [  5.88077590e-01   2.02963670e-01   8.04883190e-01   1.41355557e+00
   -5.55466730e-01  -1.34703550e+00]
 [  5.88271370e-01   2.03195090e-01   8.04874770e-01   1.24726452e+00
   -4.84054230e-01  -1.01483325e+00]
 [  6.45238020e-01   2.20660740e-01   8.11964810e-01  -7.59471000e-02
   -1.07721923e+00   3.60163620e-01]
 [  6.45236240e-01   2.20660590e-01   8.11964620e-01   1.33105908e+00
   -5.23257640e-01  -6.31993690e-01]
 [  5.80365440e-01   2.45324100e-02   6.20064840e-01  -2.45220810e-01
   -6.86604520e-01   5.12312470e-01]
 [  6.98724870e-01   2.57551200e-02   5.98090270e-01  -7.35226900e-02
   -7.80739690e-01  -2.30768820e-01]
 [  5.49510960e-01   1.91115850e-01   6.20182520e-01   2.38370620e-01
   -1.10104361e+00   3.67051620e-01]
 [  5.48877060e-01   1.90913780e-01   6.20459010e-01   1.52747020e-01
   -7.92473940e-01   4.71148720e-01]
 [  5.49504780e-01   1.91110340e-01   6.20194180e-01   1.24131345e+00
   -9.65138060e-01  -9.99111870e-01]
 [  5.49504370e-01   1.91110320e-01   6.20194350e-01   1.66061008e+00
   -4.58544140e-01  -1.18051982e+00]
 [  5.49504870e-01   1.91101200e-01   6.20186090e-01   1.36331841e+00
   -4.21041920e-01  -5.04191330e-01]
 [  6.05885020e-01   2.08404690e-01   6.09142380e-01  -5.57563860e-01
   -1.08826376e+00   5.39900860e-01]
 [  6.05884210e-01   2.08405860e-01   6.09143100e-01   1.06719771e+00
   -4.58545990e-01  -1.18052006e+00]
 [  5.98558850e-01   2.52526000e-02   4.16800390e-01   6.15381000e-03
   -7.85274280e-01   4.39976000e-03]
 [  5.15925680e-01   1.81178880e-01   4.45065330e-01   7.08647090e-01
   -1.11940030e-01  -8.83844190e-01]
 [  5.15888590e-01   1.81194200e-01   4.45041810e-01   7.10296160e-01
    1.27282010e-01  -6.79692490e-01]
 [  6.17108370e-01   2.12346670e-01   3.88706420e-01  -3.46121820e-01
   -7.97984890e-01   3.38663370e-01]
 [  6.17102530e-01   2.12297530e-01   3.88771890e-01   2.98574540e-01
   -4.99807150e-01   3.54311900e-01]
 [  6.17185010e-01   2.12361100e-01   3.88691430e-01   1.77484320e-01
   -7.72941360e-01   1.06329340e-01]
 [  6.17189290e-01   2.12363010e-01   3.88690390e-01   6.09499640e-01
   -3.03303380e-01  -2.61210280e-01]
 [  6.17203550e-01   2.12367720e-01   3.88688770e-01   1.34725861e+00
   -3.33555330e-01  -1.08657348e+00]
 [  5.45294460e-01  -1.38512300e-01   7.97788690e-01  -1.24796854e+00
   -4.81703660e-01   1.03397674e+00]
 [  5.45298030e-01  -1.38511830e-01   7.97787970e-01  -1.41337053e+00
   -5.52998200e-01   1.36586953e+00]
 [  5.45298420e-01  -1.38510890e-01   7.97787690e-01  -2.00678333e+00
   -5.52997120e-01   1.36586888e+00]
 [  5.45298990e-01  -1.38511090e-01   7.97787890e-01  -1.27335923e+00
   -1.06025734e+00   8.91602460e-01]
 [  5.45292880e-01  -1.38509850e-01   7.97786610e-01  -9.44640420e-01
   -1.03705518e+00   2.07101780e-01]
 [  5.45298810e-01  -1.38509950e-01   7.97788570e-01   1.21907833e+00
   -1.31001288e+00  -1.14660292e+00]
 [  5.45299800e-01  -1.38506880e-01   7.97785660e-01  -8.86303770e-01
   -1.88854090e+00   9.22237490e-01]
 [  6.02575120e-01  -1.54898890e-01   8.04874060e-01   1.36538380e-01
   -1.05951708e+00  -6.88968740e-01]
 [  6.02562260e-01  -1.54918160e-01   8.04873570e-01   8.92740580e-01
   -1.15983241e+00  -1.40271008e+00]
 [  6.59869510e-01  -1.71261330e-01   8.11964600e-01  -7.44939560e-01
   -7.81320720e-01   5.71530640e-01]
 [  6.59088780e-01  -1.71207200e-01   8.12552480e-01  -2.78492380e-01
   -1.69268806e+00   2.13179000e-03]
 [  5.63487640e-01  -1.43904760e-01   6.20194170e-01  -9.42793700e-01
   -9.68786670e-01   1.01621636e+00]
 [  5.64165730e-01  -1.44067790e-01   6.19851380e-01  -7.09440100e-01
   -6.05732900e-01   2.95489580e-01]
 [  5.63468040e-01  -1.43902270e-01   6.20206230e-01   6.17945000e-03
   -7.67471340e-01   1.85459900e-02]
 [  5.63486700e-01  -1.43904090e-01   6.20192960e-01  -2.73428040e-01
   -1.37830001e+00  -1.36000000e-05]
 [  5.63537600e-01  -1.43729400e-01   6.19948400e-01   1.13197777e+00
   -1.29225326e+00  -1.13343691e+00]
 [  6.20197160e-01  -1.60110540e-01   6.09140780e-01  -7.09519540e-01
   -6.05886440e-01   2.95612560e-01]
 [  6.20135340e-01  -1.60088400e-01   6.09144250e-01   2.82876880e-01
   -7.50382300e-01  -5.13074020e-01]
 [  6.20186120e-01  -1.60101900e-01   6.09132750e-01   4.19873040e-01
   -1.04632267e+00  -6.86152540e-01]
 [  6.76895420e-01  -1.76310390e-01   5.98090490e-01  -1.13606324e+00
   -2.62721720e-01   5.78688130e-01]
 [  5.29929300e-01  -1.33953400e-01   4.45053720e-01  -9.80021120e-01
   -1.85302690e-01   8.41707900e-01]
 [  5.29928690e-01  -1.33953050e-01   4.45053230e-01  -8.53827610e-01
   -7.74490960e-01   7.46121920e-01]
 [  5.30486860e-01  -1.33896760e-01   4.44364700e-01  -7.9Traceback (most recent call last):
  File "/opt/ros/indigo/lib/calibration_estimation/multi_step_cov_estimator.py", line 357, in <module>
5217050e-01
   -4.17685460e-01   1.36106430e-01]
 [  5.29843690e-01  -1.34441330e-01   4.45693850e-01  -1.31295130e-01
   -4.63867870e-01  -1.55118020e-01]
 [  5.80876180e-01  -1.48448680e-01   4.16873150e-01  -7.51968560e-01
   -3.42940680e-01   1.10248389e+00]
 [  5.80858050e-01  -1.48428340e-01   4.16897660e-01  -3.98344860e-01
   -2.26825710e-01   4.29612210e-01]
 [  5.80873620e-01  -1.48533290e-01   4.16418370e-01  -7.95207900e-01
   -4.16924050e-01   1.36051510e-01]
 [  5.80877810e-01  -1.48450020e-01   4.16871770e-01  -9.33280550e-01
   -6.23068120e-01   4.11201180e-01]
 [  5.80793000e-01  -1.48515770e-01   4.16867000e-01  -3.64501250e-01
   -7.97042480e-01  -2.17261180e-01]
 [  6.31542660e-01  -1.62760130e-01   3.88771090e-01  -2.87621300e-01
   -4.99229450e-01  -3.43004860e-01]]
Executing step without covariance calculations
    output_dict, output_poses, J = opt_runner(robot_params, previous_pose_guesses, free_dict, multisensors, use_cov)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/calibration_estimation/opt_runner.py", line 385, in opt_runner
    error_calc = ErrorCalc(robot_params, free_dict, multisensors, use_cov)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/calibration_estimation/opt_runner.py", line 53, in __init__
    self._expanded_params = robot_params.deflate()
  File "/opt/ros/indigo/lib/python2.7/dist-packages/calibration_estimation/urdf_params.py", line 238, in deflate
    deflate_primitive_dict(param_vec, self.chains)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/calibration_estimation/urdf_params.py", line 80, in deflate_primitive_dict
    param_vec[elem.start:elem.end,0] = elem.deflate()
ValueError: could not broadcast input array from shape (2,1) into shape (1,1)