#775 nesting params in yaml results file

lardemua · Dec 28, 2023 · 1603e3c · 1603e3c
1 parent 8518129
commit 1603e3c
Show file tree

Hide file tree

Showing 4 changed files with 44 additions and 34 deletions.
diff --git a/atom_calibration/scripts/calibrate b/atom_calibration/scripts/calibrate
@@ -555,10 +555,10 @@ def main():
         waitForKeyPress2(function=opt.callObjectiveFunction, timeout=0.1)
 
     print("Initializing optimization ...")
-    # options = {'ftol': 1e-3, 'xtol': 1e-3, 'gtol': 1e-3, 'diff_step': None, 'x_scale': 'jac'}
+    options = {'ftol': 1e-3, 'xtol': 1e-3, 'gtol': 1e-3, 'diff_step': None, 'x_scale': 'jac'}
     # options = {"ftol": 1e-4, "xtol": 1e-4, "gtol": 1e-4, "diff_step": None, "x_scale": "jac", }
-    options = {'ftol': 1e-5, 'xtol': 1e-5, 'gtol': 1e-5,
-               'diff_step': None, 'x_scale': 'jac'}
+    # options = {'ftol': 1e-5, 'xtol': 1e-5, 'gtol': 1e-5,
+    #    'diff_step': None, 'x_scale': 'jac'}
     # options = {'ftol': 1e-6, 'xtol': 1e-6, 'gtol': 1e-6, 'diff_step': None, 'x_scale': 'jac'}
     # options = {'ftol': 1e-7, 'xtol': 1e-7, 'gtol': 1e-7, 'diff_step': None, 'x_scale': 'jac'}
     # options = {'ftol': 1e-5, 'xtol': 1e-5, 'gtol': 1e-5,

diff --git a/atom_core/src/atom_core/results_yml_io.py b/atom_core/src/atom_core/results_yml_io.py
@@ -27,6 +27,7 @@ def saveResultsYml(dataset, selected_collection_key, output_file):
 
     # Cycle all sensors in calibration config, and for each replace the optimized transform in the original xacro
     # Parse xacro description file
+    d = {}
     for sensor_key, sensor in dataset["calibration_config"]['sensors'].items():
 
         # Search for corresponding transform. Since this is a sensor transformation it must be static, which is why we use only one collection, the selected collection key
@@ -38,22 +39,26 @@ def saveResultsYml(dataset, selected_collection_key, output_file):
                 quat = transform['quat']
                 rpy = tf.transformations.euler_from_quaternion(quat, axes='rxyz')
 
-                dict_yml[sensor_key + '_x'] = trans[0]
-                dict_yml[sensor_key + '_y'] = trans[1]
-                dict_yml[sensor_key + '_z'] = trans[2]
+                d[sensor_key] = {}
+                d[sensor_key]['x'] = float(trans[0])
+                d[sensor_key]['y'] = float(trans[1])
+                d[sensor_key]['z'] = float(trans[2])
 
-                dict_yml[sensor_key + '_roll'] = rpy[0]
-                dict_yml[sensor_key + '_pitch'] = rpy[1]
-                dict_yml[sensor_key + '_yaw'] = rpy[2]
+                d[sensor_key]['roll'] = float(rpy[0])
+                d[sensor_key]['pitch'] = float(rpy[1])
+                d[sensor_key]['yaw'] = float(rpy[2])
                 found = True
                 break
 
         if not found:
             raise atomError("Could not find transform for sensor " + sensor_key +
                             '. Cannot produce yaml file with calibration results.')
 
+    dict_yml['sensors'] = d
+
     # Cycle all additional_tfs in calibration config, and for each replace the optimized transform in the original xacro
     # Parse xacro description file
+    d = {}
     if dataset['calibration_config']['additional_tfs'] is not None:
         for additional_tf_key, additional_tf in dataset["calibration_config"]['additional_tfs'].items():
 
@@ -66,22 +71,25 @@ def saveResultsYml(dataset, selected_collection_key, output_file):
                     quat = transform['quat']
                     rpy = tf.transformations.euler_from_quaternion(quat, axes='rxyz')
 
-                    dict_yml[sensor_key + '_x'] = trans[0]
-                    dict_yml[sensor_key + '_y'] = trans[1]
-                    dict_yml[sensor_key + '_z'] = trans[2]
+                    d[sensor_key + '_x'] = float(trans[0])
+                    d[sensor_key + '_y'] = float(trans[1])
+                    d[sensor_key + '_z'] = float(trans[2])
 
-                    dict_yml[sensor_key + '_roll'] = rpy[0]
-                    dict_yml[sensor_key + '_pitch'] = rpy[1]
-                    dict_yml[sensor_key + '_yaw'] = rpy[2]
+                    d[sensor_key + '_roll'] = float(rpy[0])
+                    d[sensor_key + '_pitch'] = float(rpy[1])
+                    d[sensor_key + '_yaw'] = float(rpy[2])
                     found = True
                     break
 
             if not found:
                 raise atomError("Could not find transform for additional_tf " + additional_tf_key +
                                 '. Cannot produce yaml file with calibration results.')
 
+        dict_yml['additional_tfs'] = d
+
     # Cycle all joints in calibration config, and for each replace the optimized transform in the original xacro
     # Parse xacro description file
+    d = {}
     if dataset['calibration_config']['joints'] is not None:
         for config_joint_key, config_joint in dataset["calibration_config"]['joints'].items():
 
@@ -91,9 +99,10 @@ def saveResultsYml(dataset, selected_collection_key, output_file):
 
                 if config_joint_key == joint_key:
 
+                    d[joint_key] = {}
                     for param_to_calibrate in config_joint['params_to_calibrate']:
                         calibrated_value = dataset['collections'][selected_collection_key]['joints'][joint_key][param_to_calibrate]
-                        dict_yml[config_joint_key + '_' + param_to_calibrate] = calibrated_value
+                        d[joint_key][param_to_calibrate] = float(calibrated_value)
 
                     found = True
                     break
@@ -102,15 +111,16 @@ def saveResultsYml(dataset, selected_collection_key, output_file):
                 raise atomError("Could not find transform for additional_tf " + additional_tf_key +
                                 '. Cannot produce yaml file with calibration results.')
 
+        dict_yml['joints'] = d
     # Make sure all values in dict are float
     # https://stackoverflow.com/questions/21695705/dump-an-python-object-as-yaml-file
-    for key in dict_yml.keys():
-        dict_yml[key] = float(dict_yml[key])
+    # for key in dict_yml.keys():
+    #     dict_yml[key] = float(dict_yml[key])
 
     # DEBUG
     # for key in dict_yml.keys():  # just to verify
     #     print(key + ' is of type ' + str(type(dict_yml[key])))
     # print(dict_yml)
 
     print('Saved calibrated parameters to yaml file ' + Fore.BLUE + output_file + Style.RESET_ALL)
-    yaml.dump(dict_yml, open(output_file, 'w'))
+    yaml.dump(dict_yml, open(output_file, 'w'), sort_keys=False)
diff --git a/atom_examples/riwmpbot/riwmpbot_calibration/urdf/optimized.urdf.xacro b/atom_examples/riwmpbot/riwmpbot_calibration/urdf/optimized.urdf.xacro
@@ -402,47 +402,47 @@
     <child link="base_link_inertia"/>
   </joint>
   <joint name="shoulder_pan_joint" type="revolute">
-    <origin xyz="-0.002258933272881404 -0.005452007879327612 0.19318967649940175" rpy="-0.015076351732913353 0.0037567565236984834 0.003862401350425072"/>
+    <origin xyz="-0.0011072022674218484 0.006104470330633574 0.1776131205352634" rpy="-0.024334800436751625 -0.028227058847477848 -0.06020146484737336"/>
     <axis xyz="0.0 0.0 1.0"/>
     <parent link="base_link_inertia"/>
     <child link="shoulder_link"/>
     <limit effort="330.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="2.0943951023931953"/>
     <dynamics damping="0.0" friction="0.0"/>
   </joint>
   <joint name="shoulder_lift_joint" type="revolute">
-    <origin xyz="-0.0002721307181949406 0.0025471017252344077 -0.018156375079865218" rpy="1.5673243223322884 0.003850317754632484 0.04277095580032025"/>
+    <origin xyz="0.016431914541455865 -0.015921078340292362 0.010636931179058638" rpy="1.5415716586341253 0.014863657572835746 -0.07258247459143602"/>
     <axis xyz="0.0 0.0 1.0"/>
     <parent link="shoulder_link"/>
     <child link="upper_arm_link"/>
     <limit effort="330.0" lower="-1.8325957145940461" upper="0.0" velocity="2.0943951023931953"/>
     <dynamics damping="0.0" friction="0.0"/>
   </joint>
   <joint name="elbow_joint" type="revolute">
-    <origin xyz="-0.6229732823063552 0.011468301552393211 0.0036496054817145053" rpy="0.028450822769222242 -0.015489667469106877 -0.028710725868394724"/>
+    <origin xyz="-0.5996745659350029 -0.00048636814021923297 0.009405942102471837" rpy="-0.014553428819016492 -0.02173367222479105 0.02605134752247605"/>
     <axis xyz="0.0 0.0 1.0"/>
     <parent link="upper_arm_link"/>
     <child link="forearm_link"/>
     <limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
     <dynamics damping="0.0" friction="0.0"/>
   </joint>
   <joint name="wrist_1_joint" type="revolute">
-    <origin xyz="-0.578433010298134 0.015615992503028777 0.16693822012959603" rpy="-0.021361464338510654 -0.005027183082571024 0.00767077527438266"/>
+    <origin xyz="-0.5652841488249898 0.009018813250154935 0.17706782067685806" rpy="-0.003003155878006817 0.021644677144780536 0.005398823112075103"/>
     <axis xyz="0.0 0.0 1.0"/>
     <parent link="forearm_link"/>
     <child link="wrist_1_link"/>
     <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
     <dynamics damping="0.0" friction="0.0"/>
   </joint>
   <joint name="wrist_2_joint" type="revolute">
-    <origin xyz="0.004578767240427557 -0.11021223367714443 0.008909897334099363" rpy="1.5446759508506889 0.006531824935019179 -0.003492875866452135"/>
+    <origin xyz="0.0031153126042543534 -0.13407407955813314 -0.01778150734076564" rpy="1.5887420852926646 0.026030532780267963 0.011990836311086397"/>
     <axis xyz="0.0 0.0 1.0"/>
     <parent link="wrist_1_link"/>
     <child link="wrist_2_link"/>
     <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
     <dynamics damping="0.0" friction="0.0"/>
   </joint>
   <joint name="wrist_3_joint" type="revolute">
-    <origin xyz="-0.002822127564264542 0.10221911062284902 0.004813985937192047" rpy="1.5900694180491868 3.1650233656910447 3.1388761143970254"/>
+    <origin xyz="-0.013741436377825881 0.10116111685658188 -0.008882501308609786" rpy="1.563230653741768 3.171288905523689 3.159559083662778"/>
     <axis xyz="0.0 0.0 1.0"/>
     <parent link="wrist_2_link"/>
     <child link="wrist_3_link"/>
@@ -554,7 +554,7 @@
     <child link="tripod_center_support"/>
   </joint>
   <joint name="rgb_world_joint" type="fixed">
-    <origin xyz="0.008289981258439199 -0.015813501997259425 0.01595613024005547" rpy="-0.01120040992865985 -0.014582564928126984 0.018579125786226145"/>
+    <origin xyz="-0.01625234687483334 0.007016927088536094 0.04058858883102345" rpy="-0.0033734439638217453 -0.00856908230693528 -0.01718724775946154"/>
     <parent link="tripod_center_support"/>
     <child link="rgb_world_link"/>
   </joint>

diff --git a/atom_examples/riwmpbot/riwmpbot_calibration/urdf/optimized_w_pattern.urdf.xacro b/atom_examples/riwmpbot/riwmpbot_calibration/urdf/optimized_w_pattern.urdf.xacro
@@ -402,47 +402,47 @@
     <child link="base_link_inertia"/>
   </joint>
   <joint name="shoulder_pan_joint" type="revolute">
-    <origin xyz="-0.002258933272881404 -0.005452007879327612 0.19318967649940175" rpy="-0.015076351732913353 0.0037567565236984834 0.003862401350425072"/>
+    <origin xyz="-0.0011072022674218484 0.006104470330633574 0.1776131205352634" rpy="-0.024334800436751625 -0.028227058847477848 -0.06020146484737336"/>
     <axis xyz="0.0 0.0 1.0"/>
     <parent link="base_link_inertia"/>
     <child link="shoulder_link"/>
     <limit effort="330.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="2.0943951023931953"/>
     <dynamics damping="0.0" friction="0.0"/>
   </joint>
   <joint name="shoulder_lift_joint" type="revolute">
-    <origin xyz="-0.0002721307181949406 0.0025471017252344077 -0.018156375079865218" rpy="1.5673243223322884 0.003850317754632484 0.04277095580032025"/>
+    <origin xyz="0.016431914541455865 -0.015921078340292362 0.010636931179058638" rpy="1.5415716586341253 0.014863657572835746 -0.07258247459143602"/>
     <axis xyz="0.0 0.0 1.0"/>
     <parent link="shoulder_link"/>
     <child link="upper_arm_link"/>
     <limit effort="330.0" lower="-1.8325957145940461" upper="0.0" velocity="2.0943951023931953"/>
     <dynamics damping="0.0" friction="0.0"/>
   </joint>
   <joint name="elbow_joint" type="revolute">
-    <origin xyz="-0.6229732823063552 0.011468301552393211 0.0036496054817145053" rpy="0.028450822769222242 -0.015489667469106877 -0.028710725868394724"/>
+    <origin xyz="-0.5996745659350029 -0.00048636814021923297 0.009405942102471837" rpy="-0.014553428819016492 -0.02173367222479105 0.02605134752247605"/>
     <axis xyz="0.0 0.0 1.0"/>
     <parent link="upper_arm_link"/>
     <child link="forearm_link"/>
     <limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
     <dynamics damping="0.0" friction="0.0"/>
   </joint>
   <joint name="wrist_1_joint" type="revolute">
-    <origin xyz="-0.578433010298134 0.015615992503028777 0.16693822012959603" rpy="-0.021361464338510654 -0.005027183082571024 0.00767077527438266"/>
+    <origin xyz="-0.5652841488249898 0.009018813250154935 0.17706782067685806" rpy="-0.003003155878006817 0.021644677144780536 0.005398823112075103"/>
     <axis xyz="0.0 0.0 1.0"/>
     <parent link="forearm_link"/>
     <child link="wrist_1_link"/>
     <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
     <dynamics damping="0.0" friction="0.0"/>
   </joint>
   <joint name="wrist_2_joint" type="revolute">
-    <origin xyz="0.004578767240427557 -0.11021223367714443 0.008909897334099363" rpy="1.5446759508506889 0.006531824935019179 -0.003492875866452135"/>
+    <origin xyz="0.0031153126042543534 -0.13407407955813314 -0.01778150734076564" rpy="1.5887420852926646 0.026030532780267963 0.011990836311086397"/>
     <axis xyz="0.0 0.0 1.0"/>
     <parent link="wrist_1_link"/>
     <child link="wrist_2_link"/>
     <limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
     <dynamics damping="0.0" friction="0.0"/>
   </joint>
   <joint name="wrist_3_joint" type="revolute">
-    <origin xyz="-0.002822127564264542 0.10221911062284902 0.004813985937192047" rpy="1.5900694180491868 3.1650233656910447 3.1388761143970254"/>
+    <origin xyz="-0.013741436377825881 0.10116111685658188 -0.008882501308609786" rpy="1.563230653741768 3.171288905523689 3.159559083662778"/>
     <axis xyz="0.0 0.0 1.0"/>
     <parent link="wrist_2_link"/>
     <child link="wrist_3_link"/>
@@ -554,7 +554,7 @@
     <child link="tripod_center_support"/>
   </joint>
   <joint name="rgb_world_joint" type="fixed">
-    <origin xyz="0.008289981258439199 -0.015813501997259425 0.01595613024005547" rpy="-0.01120040992865985 -0.014582564928126984 0.018579125786226145"/>
+    <origin xyz="-0.01625234687483334 0.007016927088536094 0.04058858883102345" rpy="-0.0033734439638217453 -0.00856908230693528 -0.01718724775946154"/>
     <parent link="tripod_center_support"/>
     <child link="rgb_world_link"/>
   </joint>
@@ -703,7 +703,7 @@
     </visual>
   </link>
   <joint name="upper_arm_link-charuco_200x120_3x6" type="fixed">
-    <origin xyz="-0.23952606751593938 0.03241846079746957 0.09359130673433272" rpy="0.0003183159160325952 -0.0031434716056112813 3.1303255430084196"/>
+    <origin xyz="-0.24724257841518812 0.027060264928568997 0.09275845601095882" rpy="0.17982634684246238 -0.07152623039388409 3.114638642012699"/>
     <parent link="upper_arm_link"/>
     <child link="charuco_200x120_3x6"/>
   </joint>