25 from multiprocessing
import Pool
26 import multiprocessing
29 JOINT_NAMES = [
"LARM_JOINT0",
"LARM_JOINT1",
"LARM_JOINT2",
"LARM_JOINT3",
"LARM_JOINT4",
"LARM_JOINT5",
"LARM_JOINT6",
"LARM_JOINT7",
30 "RARM_JOINT0",
"RARM_JOINT1",
"RARM_JOINT2",
"RARM_JOINT3",
"RARM_JOINT4",
"RARM_JOINT5",
"RARM_JOINT6",
"RARM_JOINT7",
31 "HEAD_JOINT0",
"HEAD_JOINT1",
32 "CHEST_JOINT0",
"CHEST_JOINT1"]
37 for (key, val)
in variables.items():
38 format = format.replace(
"${" + key +
"}", str(val))
42 estimation_config_include_leg,
43 estimation_config_use_chain,
44 estimation_config_calibrate_joint):
48 <group ns="calibration_config" clear_params="true"> 49 <rosparam file="$(find jsk_calibration)/staro_calibration/estimate_params/test_all/${root_dir}/config/system.yaml" command="load" /> 51 <group ns="cal_steps"> 53 <group ns="staro - 00 - Estimating Checkerboard Locations"> 54 <param name="free_params" textfile="$(find jsk_calibration)/staro_calibration/estimate_params/test_all/${root_dir}/config/free_cb_locations.yaml" /> 55 <param name="use_cov" type="bool" value="False" /> 56 <rosparam if="${estimation_config_include_leg}"> 64 <rosparam unless="${estimation_config_include_leg}"> 70 <param name="output_filename" type="string" value="config_0" /> 73 <group ns="staro - 01 - Adding Camera Locations"> 74 <param name="free_params" textfile="$(find jsk_calibration)/staro_calibration/estimate_params/test_all/${root_dir}/config/free_cameras.yaml" /> 75 <param name="use_cov" type="bool" value="True" /> 76 <group if="${estimation_config_use_chain}"> 77 <rosparam if="${estimation_config_include_leg}"> 85 <rosparam unless="${estimation_config_include_leg}"> 92 <group unless="${estimation_config_use_chain}"> 98 <param name="output_filename" type="string" value="config_1" if="${estimation_config_calibrate_joint}"/> 99 <param name="output_filename" type="string" value="system_calibrated" unless="${estimation_config_calibrate_joint}"/> 101 <group ns="staro - 02 - Joint Offset" if="${estimation_config_calibrate_joint}"> 102 <param name="free_params" textfile="$(find jsk_calibration)/staro_calibration/estimate_params/test_all/${root_dir}/config/free_arms.yaml" /> 103 <param name="use_cov" type="bool" value="True" /> 112 <param name="output_filename" type="string" value="system_calibrated" /> 122 "root_dir": root_dir,
123 "estimation_config_include_leg": estimation_config_include_leg,
124 "estimation_config_use_chain": estimation_config_use_chain,
125 "estimation_config_calibrate_joint": estimation_config_calibrate_joint})
126 file_path = os.path.join(root_dir,
"estimation_config.launch")
127 with open(file_path,
"w")
as f:
128 f.write(replaced_str)
132 free_cb_locations_use_leg):
133 if free_cb_locations_use_leg:
136 LARM_chain_cb: [1, 1, 1, 1, 1, 1] 137 RARM_chain_cb: [1, 1, 1, 1, 1, 1] 138 LLEG_chain_cb: [1, 1, 1, 1, 1, 1] 139 RLEG_chain_cb: [1, 1, 1, 1, 1, 1] 143 gearing: [0, 0, 0, 0, 0, 0] 145 gearing: [0, 0, 0, 0, 0, 0] 147 gearing: [0, 0, 0, 0, 0, 0] 149 gearing: [0, 0, 0, 0, 0, 0] 173 LARM_chain_cb: [1, 1, 1, 1, 1, 1] 174 RARM_chain_cb: [1, 1, 1, 1, 1, 1] 178 gearing: [0, 0, 0, 0, 0, 0] 180 gearing: [0, 0, 0, 0, 0, 0] 182 gearing: [0, 0, 0, 0, 0, 0] 184 gearing: [0, 0, 0, 0, 0, 0] 205 if not os.path.exists(os.path.join(root_name,
"config")):
206 os.mkdir(os.path.join(root_name,
"config"))
207 file_path = os.path.join(root_name,
"config",
"free_cb_locations.yaml")
208 with open(file_path,
"w")
as f:
212 free_cameras_include_leg,
213 free_cameras_include_arm):
214 if free_cameras_include_leg
and free_cameras_include_arm:
217 LARM_chain_cb: [1, 1, 1, 1, 1, 1] 218 RARM_chain_cb: [1, 1, 1, 1, 1, 1] 219 LLEG_chain_cb: [1, 1, 1, 1, 1, 1] 220 RLEG_chain_cb: [1, 1, 1, 1, 1, 1] 221 CARMINE_joint: [1, 1, 1, 1, 1, 1] 225 gearing: [0, 0, 0, 0, 0, 0] 227 gearing: [0, 0, 0, 0, 0, 0] 229 gearing: [0, 0, 0, 0, 0, 0] 231 gearing: [0, 0, 0, 0, 0, 0] 252 elif free_cameras_include_leg
and not free_cameras_include_arm:
255 LLEG_chain_cb: [1, 1, 1, 1, 1, 1] 256 RLEG_chain_cb: [1, 1, 1, 1, 1, 1] 257 CARMINE_joint: [1, 1, 1, 1, 1, 1] 261 gearing: [0, 0, 0, 0, 0, 0] 263 gearing: [0, 0, 0, 0, 0, 0] 265 gearing: [0, 0, 0, 0, 0, 0] 267 gearing: [0, 0, 0, 0, 0, 0] 288 elif not free_cameras_include_leg
and free_cameras_include_arm:
291 LARM_chain_cb: [1, 1, 1, 1, 1, 1] 292 RARM_chain_cb: [1, 1, 1, 1, 1, 1] 293 CARMINE_joint: [1, 1, 1, 1, 1, 1] 297 gearing: [0, 0, 0, 0, 0, 0] 299 gearing: [0, 0, 0, 0, 0, 0] 301 gearing: [0, 0, 0, 0, 0, 0] 303 gearing: [0, 0, 0, 0, 0, 0] 324 elif not free_cameras_include_leg
and not free_cameras_include_arm:
327 CARMINE_joint: [1, 1, 1, 1, 1, 1] 331 gearing: [0, 0, 0, 0, 0, 0] 333 gearing: [0, 0, 0, 0, 0, 0] 335 gearing: [0, 0, 0, 0, 0, 0] 337 gearing: [0, 0, 0, 0, 0, 0] 358 file_path = os.path.join(root_dir,
"config",
"free_cameras.yaml")
359 with open(file_path,
"w")
as f:
369 LARM_chain_cb: [1, 1, 1, 1, 1, 1] 370 RARM_chain_cb: [1, 1, 1, 1, 1, 1] 371 LLEG_chain_cb: [1, 1, 1, 1, 1, 1] 372 RLEG_chain_cb: [1, 1, 1, 1, 1, 1] 373 CARMINE_joint: [0, 0, 0, 1, 1, 1] 375 if free_arms_use_arm:
376 file_str = file_str +
""" 377 LARM_JOINT0: [ 0, 0, 0, 0.0, 1.0, 0.0 ] 378 LARM_JOINT1: [ 0, 0, 0, 1.0, 0.0, 0.0 ] 379 LARM_JOINT2: [ 0, 0, 0, 0.0, 0.0, 1.0 ] 380 LARM_JOINT3: [ 0, 0, 0, 0.0, 1.0, 0.0 ] 381 LARM_JOINT4: [ 0, 0, 0, 0.0, 0.0, 1.0 ] 382 LARM_JOINT5: [ 0, 0, 0, 1.0, 0.0, 0.0 ] 383 LARM_JOINT6: [ 0, 0, 0, 0.0, 1.0, 0.0 ] 384 RARM_JOINT0: [ 0, 0, 0, 0.0, 1.0, 0.0 ] 385 RARM_JOINT1: [ 0, 0, 0, 1.0, 0.0, 0.0 ] 386 RARM_JOINT2: [ 0, 0, 0, 0.0, 0.0, 1.0 ] 387 RARM_JOINT3: [ 0, 0, 0, 0.0, 1.0, 0.0 ] 388 RARM_JOINT4: [ 0, 0, 0, 0.0, 0.0, 1.0 ] 389 RARM_JOINT5: [ 0, 0, 0, 1.0, 0.0, 0.0 ] 390 RARM_JOINT6: [ 0, 0, 0, 0.0, 1.0, 0.0 ] 392 if free_arms_use_head:
393 file_str = file_str +
""" 394 HEAD_JOINT0: [ 0, 0, 0, 0.0, 0.0, 1.0 ] 395 HEAD_JOINT1: [ 0, 0, 0, 0.0, 1.0, 0.0 ] 398 if free_arms_use_chest:
399 file_str = file_str +
""" 400 CHEST_JOINT0: [ 0, 0, 0, 0.0, 0.0, 1.0 ] 401 CHEST_JOINT1: [ 0, 0, 0, 0.0, 1.0, 0.0 ] 404 if free_arms_use_leg:
405 file_str = file_str +
""" 406 LLEG_JOINT0: [0, 0, 0, 0.0, 0.0, 1.0] 407 LLEG_JOINT1: [0, 0, 0, 1.0, 0.0, 0.0] 408 LLEG_JOINT2: [0, 0, 0, 0.0, 1.0, 0.0] 409 LLEG_JOINT3: [0, 0, 0, 0.0, 1.0, 0.0] 410 LLEG_JOINT4: [0, 0, 0, 0.0, 1.0, 0.0] 411 LLEG_JOINT5: [0, 0, 0, 1.0, 0.0, 0.0] 412 LLEG_JOINT6: [0, 0, 0, 0.0, 0.0, 0.0] 414 file_str = file_str +
""" 417 gearing: [0, 0, 0, 0, 0, 0] 419 gearing: [0, 0, 0, 0, 0, 0] 421 gearing: [0, 0, 0, 0, 0, 0] 423 gearing: [0, 0, 0, 0, 0, 0] 443 file_path = os.path.join(root_dir,
"config",
"free_arms.yaml")
444 with open(file_path,
"w")
as f:
457 joint_angles: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] 458 gearing: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] 463 joint_angles: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] 464 gearing: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] 469 joint_angles: [0.01, 0.01] 475 joint_angles: [0.01, 0.01] 479 tip: lleg_end_coords # LLEG_LINK5? 481 joint_angles: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] 482 gearing: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] 485 tip: rleg_end_coords # RLEG_LINK5? 487 joint_angles: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] 488 gearing: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] 491 chain_id: head_chain #TODO: get rid of this 492 frame_id: camera_rgb_optical_frame 493 # frame_id: HEAD_LINK1 495 # baseline_rgbd: 0.075 ## comment out if we run all_pipelines.launch with use_kinect 499 cov: {u: 0.25, v: 0.25, x: 0.25} 511 LARM_chain_cb: [-0.1, 0, 0, 0, 0, 0] 512 RARM_chain_cb: [0, 0, 0, 0, 0, 0] 513 #default_floating_initial_pose: [0.3, 0, 0.2, -1.2092, 1.2092, -1.2092] 514 #default_floating_initial_pose: [0, 0, 0, 0, 0, 0] 515 initial_poses: "/tmp/staro_calibration/config_0_poses.yaml" 516 # xyz: [-0.002956968077825933, 0.16898467170866147, -0.20050853695600393] 517 # rpy: (-0.04425102767135782, 0.01876497698337332, -0.019873022766080706) 519 with open(os.path.join(root_dir,
"config",
"system.yaml"),
"w")
as f:
524 port = counter + 11311
525 counter = counter + 1
529 rosparam delete /calibration_config 531 if [ -f robot_calibrated.xml ]; then 532 echo "robot_calibrated.xml already exists. Either back up this file or remove it before continuing" 536 echo "Checking if we can write to robot_calibrated.xml..." 537 touch robot_calibrated.xml 538 if [ "$?" -ne "0" ]; then 539 echo "Not able to write to robot_calibrated.xml" 540 echo "Make sure you run this script from a directory that for which you have write permissions." 543 rm robot_calibrated.xml 547 roslaunch estimation_config.launch 548 rosrun calibration_estimation multi_step_cov_estimator.py staro_calibration/cal_measurements.bag /tmp/staro_calibration __name:=cal_cov_estimator 552 if [ "$est_return_val" -ne "0" ]; then 553 echo "Estimator exited prematurely with error code [$est_return_val]" 557 # Make all the temporary files writable 558 chmod ag+w staro_calibration/* 561 with open(os.path.join(root_dir,
"calibrate_staro.sh"),
"w")
as f:
562 f.write(replaced_str)
563 os.chmod(os.path.join(root_dir,
"calibrate_staro.sh"), 0o0755)
566 os.mkdir(os.path.join(root_dir,
"staro_calibration"))
567 to_path = os.path.join(root_dir,
"staro_calibration",
"cal_measurements.bag")
568 shutil.copyfile(
"cal_measurements.bag", to_path)
570 def genFiles(estimation_config_include_leg,
571 estimation_config_use_chain,
572 estimation_config_calibrate_joint,
573 free_cb_locations_use_leg,
574 free_cameras_include_leg,
575 free_cameras_include_arm,
580 directory_name =
"test_%s_%s_%s_%s_%s_%s_%s_%s_%s_%s" % (estimation_config_include_leg,
581 estimation_config_use_chain,
582 estimation_config_calibrate_joint,
583 free_cb_locations_use_leg,
584 free_cameras_include_leg,
585 free_cameras_include_arm,
590 if "--cleanup" in sys.argv:
591 if os.path.exists(directory_name):
592 shutil.rmtree(directory_name)
593 os.mkdir(directory_name)
595 estimation_config_include_leg,
596 estimation_config_use_chain,
597 estimation_config_calibrate_joint)
600 free_cameras_include_arm)
615 free_workers = [
None] * parallel_num
616 for root, dirs, files
in os.walk(
'.'):
617 if "calibrate_staro.sh" in files
and "latest_calibrated_xml" not in files:
618 sh_files.append(os.path.join(root,
"calibrate_staro.sh"))
620 while len(sh_files) > 0:
621 for p, i
in zip(free_workers, range(len(free_workers))):
622 if p ==
None or p.poll() !=
None:
624 new_env = os.environ.copy()
625 new_env[
"ROS_MASTER_URI"] =
"http://localhost:%d" % (11311 + i + 1)
626 process = subprocess.Popen([sh_files.pop()], env=new_env, shell=
True)
627 free_workers[i] = process
628 print(
"%d tasks are remained" % (len(sh_files)))
635 if "--execute" in sys.argv:
638 for estimation_config_include_leg
in (
False,
True):
639 for estimation_config_use_chain
in (
False,
True):
640 for estimation_config_calibrate_joint
in (
False,
True):
641 for free_cb_locations_use_leg
in (
False,
True):
642 for free_cameras_include_leg
in (
False,
True):
643 for free_cameras_include_arm
in (
False,
True):
644 for free_arms_use_arm
in (
False,
True):
645 for free_arms_use_head
in (
False,
True):
646 for free_arms_use_chest
in (
False,
True):
647 for free_arms_use_leg
in (
False,
True):
648 genFiles(estimation_config_include_leg,
649 estimation_config_use_chain,
650 estimation_config_calibrate_joint,
651 free_cb_locations_use_leg,
652 free_cameras_include_leg,
653 free_cameras_include_arm,
659 if __name__ ==
"__main__":
def generateFreeArms(root_dir, free_arms_use_arm, free_arms_use_head, free_arms_use_chest, free_arms_use_leg)
def generateEstimationConfig(root_dir, estimation_config_include_leg, estimation_config_use_chain, estimation_config_calibrate_joint)
def generateFreeCameras(root_dir, free_cameras_include_leg, free_cameras_include_arm)
def copyBagFiles(root_dir)
def replaceStringWithDict(format, variables)
def generateSystemYaml(root_dir)
def genFiles(estimation_config_include_leg, estimation_config_use_chain, estimation_config_calibrate_joint, free_cb_locations_use_leg, free_cameras_include_leg, free_cameras_include_arm, free_arms_use_arm, free_arms_use_head, free_arms_use_chest, free_arms_use_leg)
def generateFreeCBLocations(root_name, free_cb_locations_use_leg)