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)
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__":