# Copyright [2021-2025] Thanh Nguyen
# Copyright [2022-2023] [CNRS, Toward SAS]
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
# http://www.apache.org/licenses/LICENSE-2.0
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""
Calibration tools and algorithms for robot kinematic calibration.
This module contains the implementation of calibration algorithms including:
- Forward kinematics update functions
- Levenberg-Marquardt optimization
- Base regressor calculation
- 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,
get_baseIndex,
build_baseRegressor,
)
# Import configuration functions and constants from config module
from .config import (
get_param_from_yaml,
unified_to_legacy_config,
get_sup_joints,
)
# Import parameter management functions and constants from parameter module
from .parameter import (
get_joint_offset,
get_fullparam_offset,
add_base_name,
add_pee_name,
add_eemarker_frame,
FULL_PARAMTPL,
JOINT_OFFSETTPL,
ELAS_TPL,
EE_TPL,
BASE_TPL,
)
# Import data loading functions from data_loader module
from .data_loader import (
read_config_data,
load_data,
get_idxq_from_jname,
)
# Constants for calibration
TOL_QR = 1e-8
# Re-export for backward compatibility
__all__ = [
"get_param_from_yaml",
"unified_to_legacy_config",
"get_sup_joints",
"get_joint_offset",
"get_fullparam_offset",
"add_base_name",
"add_pee_name",
"add_eemarker_frame",
"read_config_data",
"load_data",
"get_idxq_from_jname",
"cartesian_to_SE3",
"xyzquat_to_SE3",
"get_rel_transform",
"get_rel_kinreg",
"get_rel_jac",
"initialize_variables",
"update_forward_kinematics",
"calc_updated_fkm",
"update_joint_placement",
"calculate_kinematics_model",
"calculate_identifiable_kinematics_model",
"calculate_base_kinematics_regressor",
]
# COMMON TOOLS
[docs]
def cartesian_to_SE3(X):
"""Convert cartesian coordinates to SE3 transformation.
Args:
X (ndarray): (6,) array with [x,y,z,rx,ry,rz] coordinates
Returns:
pin.SE3: SE3 transformation with:
- translation from X[0:3]
- rotation matrix from RPY angles X[3:6]
"""
X = np.array(X)
X = X.flatten("C")
translation = X[0:3]
rot_matrix = pin.rpy.rpyToMatrix(X[3:6])
placement = pin.SE3(rot_matrix, translation)
return placement
[docs]
def xyzquat_to_SE3(xyzquat):
"""Convert XYZ position and quaternion orientation to SE3 transformation.
Takes a 7D vector containing XYZ position and WXYZ quaternion and creates
an SE3 transformation matrix.
Args:
xyzquat (ndarray): (7,) array containing:
- xyzquat[0:3]: XYZ position coordinates
- xyzquat[3:7]: WXYZ quaternion orientation
Returns:
pin.SE3: Rigid body transformation with:
- Translation from XYZ position
- Rotation matrix from normalized quaternion
Example:
>>> pos_quat = np.array([0.1, 0.2, 0.3, 1.0, 0, 0, 0])
>>> transform = xyzquat_to_SE3(pos_quat)
"""
xyzquat = np.array(xyzquat)
xyzquat = xyzquat.flatten("C")
translation = xyzquat[0:3]
rot_matrix = pin.Quaternion(xyzquat[3:7]).normalize().toRotationMatrix()
placement = pin.SE3(rot_matrix, translation)
return placement
[docs]
def get_rel_kinreg(model, data, start_frame, end_frame, q):
"""Calculate relative kinematic regressor between frames.
Computes frame Jacobian-based regressor matrix mapping small joint displacements
to spatial velocities.
Args:
model (pin.Model): Robot model
data (pin.Data): Robot data
start_frame (str): Starting frame name
end_frame (str): Target frame name
q (ndarray): Joint configuration vector
Returns:
ndarray: (6, 6n) regressor matrix for n joints
"""
sup_joints = get_sup_joints(model, start_frame, end_frame)
pin.framesForwardKinematics(model, data, q)
pin.updateFramePlacements(model, data)
kinreg = np.zeros((6, 6 * (model.njoints - 1)))
frame = model.frames[model.getFrameId(end_frame)]
oMf = data.oMi[frame.parent] * frame.placement
for p in sup_joints:
oMp = data.oMi[model.parents[p]] * model.jointPlacements[p]
fMp = oMf.actInv(oMp)
fXp = fMp.toActionMatrix()
kinreg[:, 6 * (p - 1) : 6 * p] = fXp
return kinreg
[docs]
def get_rel_jac(model, data, start_frame, end_frame, q):
"""Calculate relative Jacobian matrix between two frames.
Computes the difference between Jacobians of end_frame and start_frame,
giving the differential mapping from joint velocities to relative spatial velocity.
Args:
model (pin.Model): Robot model
data (pin.Data): Robot data
start_frame (str): Starting frame name
end_frame (str): Target frame name
q (ndarray): Joint configuration vector
Returns:
ndarray: (6, n) relative Jacobian matrix where:
- Rows represent [dx,dy,dz,wx,wy,wz] spatial velocities
- Columns represent joint velocities
- n is number of joints
Note:
Updates forward kinematics before computing Jacobians
"""
start_frameId = model.getFrameId(start_frame)
end_frameId = model.getFrameId(end_frame)
# update frameForwardKinematics and updateFramePlacements
pin.framesForwardKinematics(model, data, q)
pin.updateFramePlacements(model, data)
# relative Jacobian
J_start = pin.computeFrameJacobian(
model, data, q, start_frameId, pin.LOCAL
)
J_end = pin.computeFrameJacobian(model, data, q, end_frameId, pin.LOCAL)
J_rel = J_end - J_start
return J_rel
# LEVENBERG-MARQUARDT TOOLS
[docs]
def initialize_variables(calib_config, mode=0, seed=0):
"""Initialize variables for Levenberg-Marquardt optimization.
Creates initial parameter vector either as zeros or random values within bounds.
Args:
calib_config (dict): Parameter dictionary containing:
- param_name: List of parameter names to initialize
mode (int, optional): Initialization mode:
- 0: Zero initialization
- 1: Random uniform initialization. Defaults to 0.
seed (float, optional): Range [-seed,seed] for random init. Defaults to 0.
Returns:
tuple:
- var (ndarray): Initial parameter vector
- nvar (int): Number of parameters
Example:
>>> var, n = initialize_variables(params, mode=1, seed=0.1)
>>> print(var.shape)
(42,)
"""
# initialize all variables at zeros
nvar = len(calib_config["param_name"])
if mode == 0:
var = np.zeros(nvar)
elif mode == 1:
var = np.random.uniform(-seed, seed, nvar)
return var, nvar
[docs]
def update_forward_kinematics(model, data, var, q, calib_config, verbose=0):
"""Update forward kinematics with calibration parameters.
Applies geometric and kinematic error parameters to update joint placements
and compute end-effector poses. Handles:
1. Base/camera transformations
2. Joint placement offsets
3. End-effector marker frames
4. Joint elasticity effects
Args:
model (pin.Model): Robot model to update
data (pin.Data): Robot data
var (ndarray): Parameter vector matching calib_config["param_name"]
q (ndarray): Joint configurations matrix (n_samples, n_joints)
calib_config (dict): Calibration parameters containing:
- calib_model: "full_params" or "joint_offset"
- start_frame, end_frame: Frame names
- actJoint_idx: Active joint indices
- measurability: Active DOFs
verbose (int, optional): Print update info. Defaults to 0.
Returns:
ndarray: Flattened end-effector measurements for all samples
Side Effects:
- Modifies model joint placements temporarily
- Reverts model to original state before returning
"""
# read calib_config['param_name'] to allocate offset parameters to correct SE3
# convert translation: add a vector of 3 to SE3.translation
# convert orientation: convert SE3.rotation 3x3 matrix to vector rpy, add
# to vector rpy, convert back to to 3x3 matrix
# name reference of calibration parameters
if calib_config["calib_model"] == "full_params":
axis_tpl = FULL_PARAMTPL
elif calib_config["calib_model"] == "joint_offset":
axis_tpl = JOINT_OFFSETTPL
# order of joint in variables are arranged as in calib_config['actJoint_idx']
assert len(var) == len(
calib_config["param_name"]
), "Length of variables != length of params"
param_dict = dict(zip(calib_config["param_name"], var))
origin_model = model.copy()
# update model.jointPlacements
updated_params = []
start_f = calib_config["start_frame"]
end_f = calib_config["end_frame"]
# define transformation for camera frame
if calib_config["base_to_ref_frame"] is not None:
start_f = calib_config["ref_frame"]
# base frame to ref frame (i.e. Tiago: camera transformation)
base_tf = np.zeros(6)
for key in param_dict.keys():
for base_id, base_ax in enumerate(BASE_TPL):
if base_ax in key:
base_tf[base_id] = param_dict[key]
updated_params.append(key)
b_to_cam = get_rel_transform(
model, data, calib_config["start_frame"], calib_config["base_to_ref_frame"]
)
ref_to_cam = cartesian_to_SE3(base_tf)
cam_to_ref = ref_to_cam.actInv(pin.SE3.Identity())
bMo = b_to_cam * cam_to_ref
else:
if calib_config["calib_model"] == "joint_offset":
base_tf = np.zeros(6)
for key in param_dict.keys():
for base_id, base_ax in enumerate(BASE_TPL):
if base_ax in key:
base_tf[base_id] = param_dict[key]
updated_params.append(key)
bMo = cartesian_to_SE3(base_tf)
# update model.jointPlacements with joint 'full_params'/'joint_offset'
for j_id in calib_config["actJoint_idx"]:
xyz_rpy = np.zeros(6)
j_name = model.names[j_id]
for key in param_dict.keys():
if j_name in key:
# update xyz_rpy with kinematic errors
for axis_id, axis in enumerate(axis_tpl):
if axis in key:
if verbose == 1:
logger.debug(
"Updating [{}] joint placement at axis {} with [{}]".format(
j_name, axis, key
)
)
xyz_rpy[axis_id] += param_dict[key]
updated_params.append(key)
model = update_joint_placement(model, j_id, xyz_rpy)
PEE = np.zeros((calib_config["calibration_index"], calib_config["NbSample"]))
# update end_effector frame
for marker_idx in range(1, calib_config["NbMarkers"] + 1):
pee = np.zeros(6)
ee_name = "EE"
for key in param_dict.keys():
if ee_name in key and str(marker_idx) in key:
# update xyz_rpy with kinematic errors
for axis_pee_id, axis_pee in enumerate(EE_TPL):
if axis_pee in key:
if verbose == 1:
logger.debug(
"Updating [{}_{}] joint placement at axis {} with [{}]".format(
ee_name, str(marker_idx), axis_pee, key
)
)
pee[axis_pee_id] += param_dict[key]
# updated_params.append(key)
eeMf = cartesian_to_SE3(pee)
# get transform
q_ = np.copy(q)
for i in range(calib_config["NbSample"]):
pin.framesForwardKinematics(model, data, q_[i, :])
pin.updateFramePlacements(model, data)
# update model.jointPlacements with joint elastic error
if calib_config["non_geom"]:
tau = pin.computeGeneralizedGravity(
model, data, q_[i, :]
) # vector size of 32 = nq < njoints
# update xyz_rpy with joint elastic error
for j_id in calib_config["actJoint_idx"]:
xyz_rpy = np.zeros(6)
j_name = model.names[j_id]
tau_j = tau[j_id - 1] # nq = njoints -1
if j_name in key:
for elas_id, elas in enumerate(ELAS_TPL):
if elas in key:
param_dict[key] = param_dict[key] * tau_j
xyz_rpy[elas_id + 3] += param_dict[
key
] # +3 to add only on orienation
updated_params.append(key)
model = update_joint_placement(model, j_id, xyz_rpy)
# get relative transform with updated model
oMee = get_rel_transform(
model, data, calib_config["start_frame"], calib_config["end_frame"]
)
# revert model back to origin from added joint elastic error
for j_id in calib_config["actJoint_idx"]:
xyz_rpy = np.zeros(6)
j_name = model.names[j_id]
tau_j = tau[j_id - 1] # nq = njoints -1
if j_name in key:
for elas_id, elas in enumerate(ELAS_TPL):
if elas in key:
param_dict[key] = param_dict[key] * tau_j
xyz_rpy[elas_id + 3] += param_dict[
key
] # +3 to add only on orienation
updated_params.append(key)
model = update_joint_placement(model, j_id, -xyz_rpy)
else:
oMee = get_rel_transform(
model, data, calib_config["start_frame"], calib_config["end_frame"]
)
if len(updated_params) < len(param_dict):
oMf = oMee * eeMf
# final transform
trans = oMf.translation.tolist()
orient = pin.rpy.matrixToRpy(oMf.rotation).tolist()
loc = trans + orient
measure = []
for mea_id, mea in enumerate(calib_config["measurability"]):
if mea:
measure.append(loc[mea_id])
# PEE[(marker_idx-1)*calib_config['calibration_index']:marker_idx*calib_config['calibration_index'], i] = np.array(measure)
PEE[:, i] = np.array(measure)
# assert len(updated_params) == len(param_dict), "Not all parameters are updated"
PEE = PEE.flatten("C")
# revert model back to original
assert (
origin_model.jointPlacements != model.jointPlacements
), "before revert"
for j_id in calib_config["actJoint_idx"]:
xyz_rpy = np.zeros(6)
j_name = model.names[j_id]
for key in param_dict.keys():
if j_name in key:
# update xyz_rpy
for axis_id, axis in enumerate(axis_tpl):
if axis in key:
xyz_rpy[axis_id] = param_dict[key]
model = update_joint_placement(model, j_id, -xyz_rpy)
assert (
origin_model.jointPlacements != model.jointPlacements
), "after revert"
return PEE
[docs]
def calc_updated_fkm(model, data, var, q, calib_config, verbose=0):
"""Update forward kinematics with world frame transformations.
Specialized version that explicitly handles transformations between:
1. World frame to start frame (wMo)
2. Start frame to end frame (oMee)
3. End frame to marker frame (eeMf)
Args:
model (pin.Model): Robot model to update
data (pin.Data): Robot data
var (ndarray): Parameter vector matching calib_config["param_name"]
q (ndarray): Joint configurations matrix (n_samples, n_joints)
calib_config (dict): Calibration parameters containing:
- Frames and parameters from update_forward_kinematics()
- NbMarkers=1 (only supports single marker)
verbose (int, optional): Print update info. Defaults to 0.
Returns:
ndarray: Flattened marker measurements in world frame
Notes:
- Excludes joint elasticity effects
- Requires base or end-effector parameters in param_name
- Validates all parameters are properly updated
"""
# name reference of calibration parameters
if calib_config["calib_model"] == "full_params":
axis_tpl = FULL_PARAMTPL
elif calib_config["calib_model"] == "joint_offset":
axis_tpl = JOINT_OFFSETTPL
# order of joint in variables are arranged as in calib_config['actJoint_idx']
assert len(var) == len(
calib_config["param_name"]
), "Length of variables != length of params"
param_dict = dict(zip(calib_config["param_name"], var))
origin_model = model.copy()
# store parameter updated to the model
updated_params = []
# check if baseframe and end--effector frame are known
for key in param_dict.keys():
if "base" in key:
base_param_incl = True
break
else:
base_param_incl = False
for key in param_dict.keys():
if "EE" in key:
ee_param_incl = True
break
else:
ee_param_incl = False
# kinematic chain
start_f = calib_config["start_frame"]
end_f = calib_config["end_frame"]
# if world frame (measurement ref frame) to the start frame is not known,
# base_tpl needs to be used to define the first 6 parameters
# 1/ calc transformation from the world frame to start frame: wMo
if base_param_incl:
base_tf = np.zeros(6)
for key in param_dict.keys():
for base_id, base_ax in enumerate(BASE_TPL):
if base_ax in key:
base_tf[base_id] = param_dict[key]
updated_params.append(key)
wMo = cartesian_to_SE3(base_tf)
else:
wMo = pin.SE3.Identity()
# 2/ calculate transformation from the end frame to the end-effector frame,
# if not known: eeMf
if ee_param_incl and calib_config["NbMarkers"] == 1:
for marker_idx in range(1, calib_config["NbMarkers"] + 1):
pee = np.zeros(6)
ee_name = "EE"
for key in param_dict.keys():
if ee_name in key and str(marker_idx) in key:
# update xyz_rpy with kinematic errors
for axis_pee_id, axis_pee in enumerate(EE_TPL):
if axis_pee in key:
if verbose == 1:
logger.debug(
"Updating [{}_{}] joint placement at axis {} with [{}]".format(
ee_name, str(marker_idx), axis_pee, key
)
)
pee[axis_pee_id] += param_dict[key]
updated_params.append(key)
eeMf = cartesian_to_SE3(pee)
else:
if calib_config["NbMarkers"] > 1:
logger.warning("Multiple markers are not supported.")
else:
eeMf = pin.SE3.Identity()
# 3/ calculate transformation from start frame to end frame of kinematic chain using updated model: oMee
# update model.jointPlacements with kinematic error parameter
for j_id in calib_config["actJoint_idx"]:
xyz_rpy = np.zeros(6)
j_name = model.names[j_id]
# check joint name in param dict
for key in param_dict.keys():
if j_name in key:
# update xyz_rpy with kinematic errors based on identifiable axis
for axis_id, axis in enumerate(axis_tpl):
if axis in key:
if verbose == 1:
logger.debug(
"Updating [{}] joint placement at axis {} with [{}]".format(
j_name, axis, key
)
)
xyz_rpy[axis_id] += param_dict[key]
updated_params.append(key)
# updaet joint placement
model = update_joint_placement(model, j_id, xyz_rpy)
# check if all parameters are updated to the model
assert len(updated_params) == len(
list(param_dict.keys())
), "Not all parameters are updated {} and {}".format(
updated_params, list(param_dict.keys())
)
# pose vector of the end-effector
PEE = np.zeros((calib_config["calibration_index"], calib_config["NbSample"]))
q_ = np.copy(q)
for i in range(calib_config["NbSample"]):
pin.framesForwardKinematics(model, data, q_[i, :])
pin.updateFramePlacements(model, data)
# NOTE: joint elastic error is not considered in this version
oMee = get_rel_transform(model, data, start_f, end_f)
# calculate transformation from world frame to end-effector frame
wMee = wMo * oMee
wMf = wMee * eeMf
# final transform
trans = wMf.translation.tolist()
orient = pin.rpy.matrixToRpy(wMf.rotation).tolist()
loc = trans + orient
measure = []
for mea_id, mea in enumerate(calib_config["measurability"]):
if mea:
measure.append(loc[mea_id])
PEE[:, i] = np.array(measure)
# final result of updated fkm
PEE = PEE.flatten("C")
# revert model back to original
assert (
origin_model.jointPlacements != model.jointPlacements
), "before revert"
for j_id in calib_config["actJoint_idx"]:
xyz_rpy = np.zeros(6)
j_name = model.names[j_id]
for key in param_dict.keys():
if j_name in key:
# update xyz_rpy
for axis_id, axis in enumerate(axis_tpl):
if axis in key:
xyz_rpy[axis_id] = param_dict[key]
model = update_joint_placement(model, j_id, -xyz_rpy)
assert (
origin_model.jointPlacements != model.jointPlacements
), "after revert"
return PEE
[docs]
def update_joint_placement(model, joint_idx, xyz_rpy):
"""Update joint placement with offset parameters.
Modifies a joint's placement transform by adding position and orientation offsets.
Args:
model (pin.Model): Robot model to modify
joint_idx (int): Index of joint to update
xyz_rpy (ndarray): (6,) array of offsets:
- xyz_rpy[0:3]: Translation offsets (x,y,z)
- xyz_rpy[3:6]: Rotation offsets (roll,pitch,yaw)
Returns:
pin.Model: Updated robot model
Side Effects:
Modifies model.jointPlacements[joint_idx] in place
"""
tpl_translation = model.jointPlacements[joint_idx].translation
tpl_rotation = model.jointPlacements[joint_idx].rotation
tpl_orientation = pin.rpy.matrixToRpy(tpl_rotation)
# update axes
updt_translation = tpl_translation + xyz_rpy[0:3]
updt_orientation = tpl_orientation + xyz_rpy[3:6]
updt_rotation = pin.rpy.rpyToMatrix(updt_orientation)
# update placements
model.jointPlacements[joint_idx].translation = updt_translation
model.jointPlacements[joint_idx].rotation = updt_rotation
return model
# BASE REGRESSOR TOOLS
[docs]
def calculate_kinematics_model(q_i, model, data, calib_config):
"""Calculate Jacobian and kinematic regressor for single configuration.
Computes frame Jacobian and kinematic regressor matrices for tool frame
at given joint configuration.
Args:
q_i (ndarray): Joint configuration vector
model (pin.Model): Robot model
data (pin.Data): Robot data
calib_config (dict): Parameters containing "IDX_TOOL" frame index
Returns:
tuple:
- model (pin.Model): Updated model
- data (pin.Data): Updated data
- R (ndarray): (6,6n) Kinematic regressor matrix
- J (ndarray): (6,n) Frame Jacobian matrix
"""
pin.forwardKinematics(model, data, q_i)
pin.updateFramePlacements(model, data)
J = pin.computeFrameJacobian(
model, data, q_i, calib_config["IDX_TOOL"], pin.LOCAL
)
R = pin.computeFrameKinematicRegressor(
model, data, calib_config["IDX_TOOL"], pin.LOCAL
)
return model, data, R, J
[docs]
def calculate_identifiable_kinematics_model(q, model, data, calib_config):
"""Calculate identifiable Jacobian and regressor matrices.
Builds aggregated Jacobian and regressor matrices from either:
1. Given set of configurations, or
2. Random configurations if none provided
Args:
q (ndarray, optional): Joint configurations matrix. If empty, uses random configs.
model (pin.Model): Robot model
data (pin.Data): Robot data
calib_config (dict): Parameters containing:
- NbSample: Number of configurations
- calibration_index: Number of active DOFs
- start_frame, end_frame: Frame names
- calib_model: Model type
Returns:
ndarray: Either:
- Joint offset case: Frame Jacobian matrix
- Full params case: Kinematic regressor matrix
Note:
Removes rows corresponding to inactive DOFs and zero elements
"""
q_temp = np.copy(q)
# Note if no q id given then use random generation of q to determine the
# minimal kinematics model
if np.any(q):
MIN_MODEL = 0
else:
MIN_MODEL = 1
# obtain aggreated Jacobian matrix J and kinematic regressor R
calib_idx = calib_config["calibration_index"]
R = np.zeros([6 * calib_config["NbSample"], 6 * (model.njoints - 1)])
J = np.zeros([6 * calib_config["NbSample"], model.njoints - 1])
for i in range(calib_config["NbSample"]):
if MIN_MODEL == 1:
q_rand = pin.randomConfiguration(model)
q_i = calib_config["q0"]
q_i[calib_config["config_idx"]] = q_rand[calib_config["config_idx"]]
else:
q_i = q_temp[i, :]
if calib_config["start_frame"] == "universe":
model, data, Ri, Ji = calculate_kinematics_model(
q_i, model, data, calib_config
)
else:
Ri = get_rel_kinreg(
model, data, calib_config["start_frame"], calib_config["end_frame"], q_i
)
# Ji = np.zeros([6, model.njoints-1]) ## TODO: get_rel_jac
Ji = get_rel_jac(
model, data, calib_config["start_frame"], calib_config["end_frame"], q_i
)
for j, state in enumerate(calib_config["measurability"]):
if state:
R[calib_config["NbSample"] * j + i, :] = Ri[j, :]
J[calib_config["NbSample"] * j + i, :] = Ji[j, :]
# remove zero rows
zero_rows = []
for r_idx in range(R.shape[0]):
if np.linalg.norm(R[r_idx, :]) < 1e-6:
zero_rows.append(r_idx)
R = np.delete(R, zero_rows, axis=0)
zero_rows = []
for r_idx in range(J.shape[0]):
if np.linalg.norm(J[r_idx, :]) < 1e-6:
zero_rows.append(r_idx)
J = np.delete(J, zero_rows, axis=0)
# select regressor matrix based on calibration model
if calib_config["calib_model"] == "joint_offset":
return J
elif calib_config["calib_model"] == "full_params":
return R
[docs]
def calculate_base_kinematics_regressor(q, model, data, calib_config, tol_qr=TOL_QR):
"""Calculate base regressor matrix for calibration parameters.
Identifies base (identifiable) parameters by:
1. Computing regressors with random/given configurations
2. Eliminating unidentifiable parameters
3. Finding independent regressor columns
Args:
q (ndarray): Joint configurations matrix
model (pin.Model): Robot model
data (pin.Data): Robot data
calib_config (dict): Contains calibration settings:
- free_flyer: Whether base is floating
- calib_model: Either "joint_offset" or "full_params"
tol_qr (float, optional): QR decomposition tolerance. Defaults to TOL_QR.
Returns:
tuple:
- Rrand_b (ndarray): Base regressor from random configs
- R_b (ndarray): Base regressor from given configs
- R_e (ndarray): Full regressor after eliminating unidentifiable params
- paramsrand_base (list): Names of base parameters from random configs
- paramsrand_e (list): Names of identifiable parameters
Side Effects:
- Updates calib_config["param_name"] with identified base parameters
- Prints regressor matrix shapes
"""
# obtain joint names
joint_names = [name for i, name in enumerate(model.names[1:])]
geo_params = get_fullparam_offset(joint_names)
joint_offsets = get_joint_offset(model, joint_names)
# calculate kinematic regressor with random configs
if not calib_config["free_flyer"]:
Rrand = calculate_identifiable_kinematics_model([], model, data, calib_config)
else:
Rrand = calculate_identifiable_kinematics_model(q, model, data, calib_config)
# calculate kinematic regressor with input configs
if np.any(np.array(q)):
R = calculate_identifiable_kinematics_model(q, model, data, calib_config)
else:
R = Rrand
# only joint offset parameters
if calib_config["calib_model"] == "joint_offset":
geo_params_sel = joint_offsets
# select columns corresponding to joint_idx
Rrand_sel = Rrand
# select columns corresponding to joint_idx
R_sel = R
# full 6 parameters
elif calib_config["calib_model"] == "full_params":
geo_params_sel = geo_params
Rrand_sel = Rrand
R_sel = R
# remove non affect columns from random data => reduced regressor
Rrand_e, paramsrand_e = eliminate_non_dynaffect(
Rrand_sel, geo_params_sel, tol_e=1e-6
)
# indices of independent columns (base param) w.r.t to reduced regressor
idx_base = get_baseIndex(Rrand_e, paramsrand_e, tol_qr=tol_qr)
# get base regressor and base params from random data
Rrand_b, paramsrand_base, _ = get_baseParams(
Rrand_e, paramsrand_e, tol_qr=tol_qr
)
# remove non affect columns from GIVEN data
R_e, params_e = eliminate_non_dynaffect(R_sel, geo_params_sel, tol_e=1e-6)
# get base param from given data
# idx_gbase = get_baseIndex(R_e, params_e, tol_qr=tol_qr)
R_gb, params_gbase, _ = get_baseParams(R_e, params_e, tol_qr=tol_qr)
# get base regressor from GIVEN data
R_b = build_baseRegressor(R_e, idx_base)
# update calibrating calib_config['param_name']/calibrating parameters
for j in idx_base:
calib_config["param_name"].append(paramsrand_e[j])
return Rrand_b, R_b, R_e, paramsrand_base, paramsrand_e