Merge pull request #8 from thanhndv212/devel

Logging, Pinocchio 3.x Compatibility & Trajectory Framework Refactoring

Author: thanhndv212

src/figaroh/calibration/base_calibration.py

@@ -23,9 +23,14 @@
 - Data loading and processing utilities
 """
 
+import logging
 import numpy as np
 import pinocchio as pin
 
+# Setup logger for this module
+logger = logging.getLogger(__name__)
+logger.addHandler(logging.NullHandler())
+
 from ..tools.regressor import eliminate_non_dynaffect
 from ..tools.qrdecomposition import (
     get_baseParams,
@@ -361,7 +366,7 @@ def update_forward_kinematics(model, data, var, q, calib_config, verbose=0):
                 for axis_id, axis in enumerate(axis_tpl):
                     if axis in key:
                         if verbose == 1:
-                            print(
+                            logger.debug(
                                 "Updating [{}] joint placement at axis {} with [{}]".format(
                                     j_name, axis, key
                                 )
@@ -381,7 +386,7 @@ def update_forward_kinematics(model, data, var, q, calib_config, verbose=0):
                 for axis_pee_id, axis_pee in enumerate(EE_TPL):
                     if axis_pee in key:
                         if verbose == 1:
-                            print(
+                            logger.debug(
                                 "Updating [{}_{}] joint placement at axis {} with [{}]".format(
                                     ee_name, str(marker_idx), axis_pee, key
                                 )
@@ -568,7 +573,7 @@ def calc_updated_fkm(model, data, var, q, calib_config, verbose=0):
                     for axis_pee_id, axis_pee in enumerate(EE_TPL):
                         if axis_pee in key:
                             if verbose == 1:
-                                print(
+                                logger.debug(
                                     "Updating [{}_{}] joint placement at axis {} with [{}]".format(
                                         ee_name, str(marker_idx), axis_pee, key
                                     )
@@ -579,7 +584,7 @@ def calc_updated_fkm(model, data, var, q, calib_config, verbose=0):
             eeMf = cartesian_to_SE3(pee)
     else:
         if calib_config["NbMarkers"] > 1:
-            print("Multiple markers are not supported.")
+            logger.warning("Multiple markers are not supported.")
         else:
             eeMf = pin.SE3.Identity()
 
@@ -598,7 +603,7 @@ def calc_updated_fkm(model, data, var, q, calib_config, verbose=0):
                 for axis_id, axis in enumerate(axis_tpl):
                     if axis in key:
                         if verbose == 1:
-                            print(
+                            logger.debug(
                                 "Updating [{}] joint placement at axis {} with [{}]".format(
                                     j_name, axis, key
                                 )

src/figaroh/calibration/calibration_tools.py

@@ -23,6 +23,12 @@
 - Frame and joint configuration management
 """
 
+import logging
+
+# Setup logger for this module
+logger = logging.getLogger(__name__)
+logger.addHandler(logging.NullHandler())
+
 
 def get_sup_joints(model, start_frame, end_frame):
     """Get list of supporting joints between two frames in kinematic chain.
@@ -50,8 +56,8 @@ def get_sup_joints(model, start_frame, end_frame):
     """
     start_frameId = model.getFrameId(start_frame)
     end_frameId = model.getFrameId(end_frame)
-    start_par = model.frames[start_frameId].parent
-    end_par = model.frames[end_frameId].parent
+    start_par = model.frames[start_frameId].parentJoint
+    end_par = model.frames[end_frameId].parentJoint
     branch_s = model.supports[start_par].tolist()
     branch_e = model.supports[end_par].tolist()
     # remove 'universe' joint from branches
@@ -165,7 +171,7 @@ def get_param_from_yaml(robot, calib_data) -> dict:
     except KeyError:
         base_to_ref_frame = None
         ref_frame = None
-        print("base_to_ref_frame and ref_frame are not defined.")
+        logger.warning("base_to_ref_frame and ref_frame are not defined.")
 
     # Validate base-to-ref frame if provided
     if base_to_ref_frame is not None:
@@ -189,7 +195,7 @@ def get_param_from_yaml(robot, calib_data) -> dict:
     except KeyError:
         base_pose = None
         tip_pose = None
-        print("base_pose and tip_pose are not defined.")
+        logger.warning("base_pose and tip_pose are not defined.")
 
     calib_config["base_pose"] = base_pose
     calib_config["tip_pose"] = tip_pose
@@ -203,7 +209,7 @@ def get_param_from_yaml(robot, calib_data) -> dict:
     calib_config["IDX_TOOL"] = IDX_TOOL
 
     # tool_joint: ID of the joint right before the tool's frame (parent)
-    tool_joint = robot.model.frames[IDX_TOOL].parent
+    tool_joint = robot.model.frames[IDX_TOOL].parentJoint
     calib_config["tool_joint"] = tool_joint
 
     # indices of active joints: from base to tool_joint
@@ -462,7 +468,7 @@ def _extract_poses(calib_config, measurements):
     calib_config["tip_pose"] = tip_pose
 
     if not base_pose and not tip_pose:
-        print("Warning: base_pose and tip_pose are not defined.")
+        logger.warning("base_pose and tip_pose are not defined.")
 
 
 def _extract_calibration_params(calib_config, robot, parameters):

src/figaroh/calibration/config.py

@@ -24,9 +24,14 @@
 - Configuration vector management
 """
 
+import logging
 import numpy as np
 import pandas as pd
 
+# Setup logger for this module
+logger = logging.getLogger(__name__)
+logger.addHandler(logging.NullHandler())
+
 
 # Export public API
 __all__ = [
@@ -131,7 +136,7 @@ def load_data(path_to_file, model, calib_config, del_list=[]):
     csv_headers = list(df.columns)
     for header in PEE_headers + joint_headers:
         if header not in csv_headers:
-            print("%s does not exist in the file." % header)
+            logger.warning("%s does not exist in the file.", header)
             break
 
     # Extract marker position/location