# 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.
import numpy as np
import pinocchio as pin
import pprint
import yaml
from yaml.loader import SafeLoader
from os.path import abspath
import matplotlib.pyplot as plt
# import quadprog as qp
import pandas as pd
from ..tools.regressor import eliminate_non_dynaffect
from ..tools.qrdecomposition import (
get_baseParams,
get_baseIndex,
build_baseRegressor,
)
TOL_QR = 1e-8
FULL_PARAMTPL = ["d_px", "d_py", "d_pz", "d_phix", "d_phiy", "d_phiz"]
JOINT_OFFSETTPL = [
"offsetPX",
"offsetPY",
"offsetPZ",
"offsetRX",
"offsetRY",
"offsetRZ",
]
ELAS_TPL = [
"k_PX",
"k_PY",
"k_PZ",
"k_RX",
"k_RY",
"k_RZ",
] # ONLY REVOLUTE JOINT FOR NOW
EE_TPL = ["pEEx", "pEEy", "pEEz", "phiEEx", "phiEEy", "phiEEz"]
BASE_TPL = [
"base_px",
"base_py",
"base_pz",
"base_phix",
"base_phiy",
"base_phiz",
]
# INITIALIZATION TOOLS
[docs]
def get_param_from_yaml(robot, calib_data) -> dict:
"""Parse calibration parameters from YAML configuration file.
Processes robot and calibration data to build a parameter dictionary containing
all necessary settings for robot calibration. Handles configuration of:
- Frame identifiers and relationships
- Marker/measurement settings
- Joint indices and configurations
- Non-geometric parameters
- Eye-hand calibration setup
Args:
robot (pin.RobotWrapper): Robot instance containing model and data
calib_data (dict): Calibration parameters parsed from YAML file containing:
- markers: List of marker configurations
- calib_level: Calibration model type
- base_frame: Starting frame name
- tool_frame: End frame name
- free_flyer: Whether base is floating
- non_geom: Whether to include non-geometric params
Returns:
dict: Parameter dictionary containing:
- robot_name: Name of robot model
- NbMarkers: Number of markers
- measurability: Measurement DOFs per marker
- start_frame, end_frame: Frame names
- base_to_ref_frame: Optional camera frame
- IDX_TOOL: Tool frame index
- actJoint_idx: Active joint indices
- param_name: List of parameter names
- Additional settings from YAML
Side Effects:
Prints warning messages if optional frames undefined
Prints final parameter dictionary
Example:
>>> calib_data = yaml.safe_load(config_file)
>>> params = get_param_from_yaml(robot, calib_data)
>>> print(params['NbMarkers'])
2
"""
# NOTE: since joint 0 is universe and it is trivial,
# indices of joints are different from indices of joint configuration,
# different from indices of joint velocities
param = dict()
robot_name = robot.model.name
frames = [f.name for f in robot.model.frames]
param["robot_name"] = robot_name
# End-effector sensing measurability:
NbMarkers = len(calib_data["markers"])
measurability = calib_data["markers"][0]["measure"]
calib_idx = measurability.count(True)
param["NbMarkers"] = NbMarkers
param["measurability"] = measurability
param["calibration_index"] = calib_idx
# Calibration model
param["calib_model"] = calib_data["calib_level"]
# Get start and end frames
start_frame = calib_data["base_frame"]
end_frame = calib_data["tool_frame"]
# Validate frames exist
err_msg = "{}_frame {} does not exist"
if start_frame not in frames:
raise AssertionError(err_msg.format("Start", start_frame))
if end_frame not in frames:
raise AssertionError(err_msg.format("End", end_frame))
param["start_frame"] = start_frame
param["end_frame"] = end_frame
# Handle eye-hand calibration frames
try:
base_to_ref_frame = calib_data["base_to_ref_frame"]
ref_frame = calib_data["ref_frame"]
except KeyError:
base_to_ref_frame = None
ref_frame = None
print("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:
if base_to_ref_frame not in frames:
err_msg = "base_to_ref_frame {} does not exist"
raise AssertionError(err_msg.format(base_to_ref_frame))
# Validate ref frame if provided
if ref_frame is not None:
if ref_frame not in frames:
err_msg = "ref_frame {} does not exist"
raise AssertionError(err_msg.format(ref_frame))
param["base_to_ref_frame"] = base_to_ref_frame
param["ref_frame"] = ref_frame
# Get initial poses
try:
camera_pose = calib_data["camera_pose"]
tip_pose = calib_data["tip_pose"]
except KeyError:
camera_pose = None
tip_pose = None
print("camera_pose and tip_pose are not defined.")
param["camera_pose"] = camera_pose
param["tip_pose"] = tip_pose
# q0: default zero configuration
param["q0"] = robot.q0
param["NbSample"] = calib_data["nb_sample"]
# IDX_TOOL: frame ID of the tool
IDX_TOOL = robot.model.getFrameId(end_frame)
param["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
param["tool_joint"] = tool_joint
# indices of active joints: from base to tool_joint
actJoint_idx = get_sup_joints(robot.model, start_frame, end_frame)
param["actJoint_idx"] = actJoint_idx
# indices of joint configuration corresponding to active joints
config_idx = [robot.model.joints[i].idx_q for i in actJoint_idx]
param["config_idx"] = config_idx
# number of active joints
NbJoint = len(actJoint_idx)
param["NbJoint"] = NbJoint
# initialize a list of calibrating parameters name
param_name = []
if calib_data["non_geom"]:
# list of elastic gain parameter names
elastic_gain = []
joint_axes = ["PX", "PY", "PZ", "RX", "RY", "RZ"]
for j_id, joint_name in enumerate(robot.model.names.tolist()):
if joint_name == "universe":
axis_motion = "null"
else:
# for ii, ax in enumerate(AXIS_MOTION[j_id]):
# if ax == 1:
# axis_motion = axis[ii]
shortname = robot.model.joints[
j_id
].shortname() # ONLY TAKE PRISMATIC AND REVOLUTE JOINT
for ja in joint_axes:
if ja in shortname:
axis_motion = ja
elif "RevoluteUnaligned" in shortname:
axis_motion = "RZ" # hard coded fix for canopies
elastic_gain.append("k_" + axis_motion + "_" + joint_name)
for i in actJoint_idx:
param_name.append(elastic_gain[i])
param["param_name"] = param_name
param.update(
{
"free_flyer": calib_data["free_flyer"],
"non_geom": calib_data["non_geom"],
"eps": 1e-3,
"PLOT": 0,
}
)
try:
param.update(
{
"coeff_regularize": calib_data["coeff_regularize"],
"data_file": calib_data["data_file"],
"sample_configs_file": calib_data["sample_configs_file"],
"outlier_eps": calib_data["outlier_eps"],
}
)
except KeyError:
param.update(
{
"coeff_regularize": None,
"data_file": None,
"sample_configs_file": None,
"outlier_eps": None,
}
)
pprint.pprint(param)
return param
[docs]
def get_joint_offset(model, joint_names):
"""Get dictionary of joint offset parameters.
Maps joint names to their offset parameters, handling special cases for
different joint types and multiple DOF joints.
Args:
model: Pinocchio robot model
joint_names: List of joint names from model.names
Returns:
dict: Mapping of joint offset parameter names to initial zero values.
Keys have format: "{offset_type}_{joint_name}"
Example:
>>> offsets = get_joint_offset(robot.model, robot.model.names[1:])
>>> print(offsets["offsetRZ_joint1"])
0.0
"""
joint_off = []
joint_names = list(model.names[1:])
joints = list(model.joints[1:])
assert len(joint_names) == len(
joints
), "Number of jointnames does not match number of joints! Please check\
imported model."
for id, joint in enumerate(joints):
name = joint_names[id]
shortname = joint.shortname()
if model.name == "canopies":
if "RevoluteUnaligned" in shortname:
shortname = shortname.replace("RevoluteUnaligned", "RZ")
for i in range(joint.nv):
if i > 0:
offset_param = (
shortname.replace("JointModel", "offset")
+ "{}".format(i + 1)
+ "_"
+ name
)
else:
offset_param = (
shortname.replace("JointModel", "offset") + "_" + name
)
joint_off.append(offset_param)
phi_jo = [0] * len(joint_off) # default zero values
joint_off = dict(zip(joint_off, phi_jo))
return joint_off
[docs]
def get_geo_offset(joint_names):
"""Get dictionary of geometric parameter variations.
Creates mapping of geometric offset parameters for each joint's
position and orientation.
Args:
joint_names: List of joint names from robot model
Returns:
dict: Mapping of geometric parameter names to initial zero values.
Keys have format: "d_{param}_{joint_name}" where param is:
- px, py, pz: Position offsets
- phix, phiy, phiz: Orientation offsets
Example:
>>> geo_params = get_geo_offset(robot.model.names[1:])
>>> print(geo_params["d_px_joint1"])
0.0
"""
tpl_names = ["d_px", "d_py", "d_pz", "d_phix", "d_phiy", "d_phiz"]
geo_params = []
for i in range(len(joint_names)):
for j in tpl_names:
# geo_params.append(j + ("_%d" % i))
geo_params.append(j + "_" + joint_names[i])
phi_gp = [0] * len(geo_params) # default zero values
geo_params = dict(zip(geo_params, phi_gp))
return geo_params
[docs]
def add_base_name(param):
"""Add base frame parameters to parameter list.
Updates param["param_name"] with base frame parameters depending on
calibration model type.
Args:
param: Parameter dictionary containing:
- calib_model: "full_params" or "joint_offset"
- param_name: List of parameter names to update
Side Effects:
Modifies param["param_name"] in place by:
- For full_params: Replaces first 6 entries with base parameters
- For joint_offset: Prepends base parameters to list
"""
if param["calib_model"] == "full_params":
param["param_name"][0:6] = BASE_TPL
elif param["calib_model"] == "joint_offset":
param["param_name"] = BASE_TPL + param["param_name"]
[docs]
def add_pee_name(param):
"""Add end-effector marker parameters to parameter list.
Adds parameters for each active measurement DOF of each marker.
Args:
param: Parameter dictionary containing:
- NbMarkers: Number of markers
- measurability: List of booleans for active DOFs
- param_name: List of parameter names to update
Side Effects:
Modifies param["param_name"] in place by appending marker parameters
in format: "{param_type}_{marker_num}"
"""
PEE_names = []
for i in range(param["NbMarkers"]):
for j, state in enumerate(param["measurability"]):
if state:
PEE_names.extend(["{}_{}".format(EE_TPL[j], i + 1)])
param["param_name"] = param["param_name"] + PEE_names
[docs]
def add_eemarker_frame(frame_name, p, rpy, model, data):
"""Add a new frame attached to the end-effector.
Creates and adds a fixed frame to the robot model at the end-effector location,
typically used for marker or tool frames.
Args:
frame_name (str): Name for the new frame
p (ndarray): 3D position offset from parent frame
rpy (ndarray): Roll-pitch-yaw angles for frame orientation
model (pin.Model): Robot model to add frame to
data (pin.Data): Robot data structure
Returns:
int: ID of newly created frame
Note:
Currently hardcoded to attach to "arm_7_joint". This should be made
configurable in future versions.
"""
p = np.array([0.1, 0.1, 0.1])
R = pin.rpy.rpyToMatrix(rpy)
frame_placement = pin.SE3(R, p)
parent_jointId = model.getJointId("arm_7_joint")
prev_frameId = model.getFrameId("arm_7_joint")
ee_frame_id = model.addFrame(
pin.Frame(
frame_name,
parent_jointId,
prev_frameId,
frame_placement,
pin.FrameType(0),
pin.Inertia.Zero(),
),
False,
)
return ee_frame_id
# DATA PROCESSING TOOLS
# data collected and saved in various formats and structure, it has
# always been a pain in the ass to handles. Need to think a way to simplify
# and optimize this procedure.
[docs]
def read_config_data(model, path_to_file):
"""Read joint configurations from CSV file.
Args:
model (pin.Model): Robot model containing joint information
path_to_file (str): Path to CSV file containing joint configurations
Returns:
ndarray: Matrix of shape (n_samples, n_joints-1) containing joint positions
"""
df = pd.read_csv(path_to_file)
q = np.zeros([len(df), model.njoints - 1])
for i in range(len(df)):
for j, name in enumerate(model.names[1:].tolist()):
jointidx = rank_in_configuration(model, name)
q[i, jointidx] = df[name][i]
return q
[docs]
def load_data(path_to_file, model, param, del_list=[]):
"""Load joint configuration and marker data from CSV file.
Reads marker positions/orientations and joint configurations from a CSV file.
Handles data validation, bad sample removal, and conversion to numpy arrays.
Args:
path_to_file (str): Path to CSV file containing recorded data
model (pin.Model): Robot model containing joint information
param (dict): Parameter dictionary containing:
- NbMarkers: Number of markers to load
- measurability: List indicating which DOFs are measured
- actJoint_idx: List of active joint indices
- config_idx: Configuration vector indices
- q0: Default configuration vector
del_list (list, optional): Indices of bad samples to remove. Defaults to [].
Returns:
tuple:
- PEEm_exp (ndarray): Flattened marker measurements of shape (n_active_dofs,)
- q_exp (ndarray): Joint configurations of shape (n_samples, n_joints)
Note:
CSV file must contain columns:
- For each marker i: [xi, yi, zi, phixi, phiyi, phizi]
- Joint names matching model.names for active joints
Raises:
KeyError: If required columns are missing from CSV
Side Effects:
- Prints joint headers
- Updates param["NbSample"] with number of valid samples
"""
# read_csv
df = pd.read_csv(path_to_file)
# create headers for marker position
PEE_headers = []
pee_tpl = ["x", "y", "z", "phix", "phiy", "phiz"]
for i in range(param["NbMarkers"]):
for j, state in enumerate(param["measurability"]):
if state:
PEE_headers.extend(["{}{}".format(pee_tpl[j], i + 1)])
# create headers for joint configurations
joint_headers = [model.names[i] for i in param["actJoint_idx"]]
print(joint_headers)
# check if all created headers present in csv file
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)
break
# Extract marker position/location
xyz_4Mkr = df[PEE_headers].to_numpy()
# Extract joint configurations
q_act = df[joint_headers].to_numpy()
# remove bad data
if del_list:
xyz_4Mkr = np.delete(xyz_4Mkr, del_list, axis=0)
q_act = np.delete(q_act, del_list, axis=0)
# update number of data points
param["NbSample"] = q_act.shape[0]
PEEm_exp = xyz_4Mkr.T
PEEm_exp = PEEm_exp.flatten("C")
q_exp = np.empty((param["NbSample"], param["q0"].shape[0]))
for i in range(param["NbSample"]):
config = param["q0"]
config[param["config_idx"]] = q_act[i, :]
q_exp[i, :] = config
return PEEm_exp, q_exp
# COMMON TOOLS
[docs]
def rank_in_configuration(model, joint_name):
"""Get index of joint in configuration vector.
Args:
model (pin.Model): Robot model
joint_name (str): Name of joint to find index for
Returns:
int: Index of joint in configuration vector q
Raises:
AssertionError: If joint name does not exist in model
"""
assert joint_name in model.names, "Given joint name does not exist."
jointId = model.getJointId(joint_name)
joint_idx = model.joints[jointId].idx_q
return joint_idx
[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_sup_joints(model, start_frame, end_frame):
"""Get list of supporting joints between two frames in kinematic chain.
Finds all joints that contribute to relative motion between start_frame and
end_frame by analyzing their support branches in the kinematic tree.
Args:
model (pin.Model): Robot model
start_frame (str): Name of starting frame
end_frame (str): Name of ending frame
Returns:
list[int]: Joint IDs ordered from start to end frame, handling cases:
1. Branch entirely contained in another branch
2. Disjoint branches with fixed root
3. Partially overlapping branches
Raises:
AssertionError: If end frame appears before start frame in chain
Note:
Excludes "universe" joints from returned list since they don't
contribute to relative motion.
"""
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
branch_s = model.supports[start_par].tolist()
branch_e = model.supports[end_par].tolist()
# remove 'universe' joint from branches
if model.names[branch_s[0]] == "universe":
branch_s.remove(branch_s[0])
if model.names[branch_e[0]] == "universe":
branch_e.remove(branch_e[0])
# find over-lapping joints in two branches
shared_joints = list(set(branch_s) & set(branch_e))
# create a list of supporting joints between two frames
list_1 = [x for x in branch_s if x not in branch_e]
list_1.reverse()
list_2 = [y for y in branch_e if y not in branch_s]
# case 2: root_joint is fixed; branch_s and branch_e are separate
if shared_joints == []:
sup_joints = list_1 + list_2
else:
# case 1: branch_s is part of branch_e
if shared_joints == branch_s:
sup_joints = [branch_s[-1]] + list_2
else:
assert (
shared_joints != branch_e
), "End frame should be before start frame."
# case 3: there are overlapping joints between two branches
sup_joints = list_1 + [shared_joints[-1]] + list_2
return sup_joints
[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 get_LMvariables(param, mode=0, seed=0):
"""Initialize variables for Levenberg-Marquardt optimization.
Creates initial parameter vector either as zeros or random values within bounds.
Args:
param (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 = get_LMvariables(params, mode=1, seed=0.1)
>>> print(var.shape)
(42,)
"""
# initialize all variables at zeros
nvar = len(param["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, param, 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 param["param_name"]
q (ndarray): Joint configurations matrix (n_samples, n_joints)
param (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 param['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 param["calib_model"] == "full_params":
axis_tpl = FULL_PARAMTPL
elif param["calib_model"] == "joint_offset":
axis_tpl = JOINT_OFFSETTPL
# order of joint in variables are arranged as in param['actJoint_idx']
assert len(var) == len(
param["param_name"]
), "Length of variables != length of params"
param_dict = dict(zip(param["param_name"], var))
origin_model = model.copy()
# update model.jointPlacements
updated_params = []
start_f = param["start_frame"]
end_f = param["end_frame"]
# define transformation for camera frame
if param["base_to_ref_frame"] is not None:
start_f = param["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, param["start_frame"], param["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 param["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 param["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:
print(
"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((param["calibration_index"], param["NbSample"]))
# update end_effector frame
for marker_idx in range(1, param["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:
print(
"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(param["NbSample"]):
pin.framesForwardKinematics(model, data, q_[i, :])
pin.updateFramePlacements(model, data)
# update model.jointPlacements with joint elastic error
if param["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 param["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, param["start_frame"], param["end_frame"]
)
# revert model back to origin from added joint elastic error
for j_id in param["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, param["start_frame"], param["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(param["measurability"]):
if mea:
measure.append(loc[mea_id])
# PEE[(marker_idx-1)*param['calibration_index']:marker_idx*param['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 param["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_forward_kinematics_2(model, data, var, q, param, verbose=0):
"""Update forward kinematics with multiple markers.
Similar to update_forward_kinematics() but handles multiple end-effector
markers. Each marker can have its own frame transformation.
Args:
model (pin.Model): Robot model to update
data (pin.Data): Robot data
var (ndarray): Parameter vector matching param["param_name"]
q (ndarray): Joint configurations matrix (n_samples, n_joints)
param (dict): Calibration parameters containing:
- NbMarkers: Number of markers
- calibration_index: Active DOFs per marker
- Additional params from update_forward_kinematics()
verbose (int, optional): Print update info. Defaults to 0.
Returns:
ndarray: Concatenated marker measurements for all samples
Raises:
AssertionError: If number of markers doesn't match frames
"""
if param["calib_model"] == "full_params":
axis_tpl = FULL_PARAMTPL
elif param["calib_model"] == "joint_offset":
axis_tpl = JOINT_OFFSETTPL
# order of joint in variables are arranged as in param['actJoint_idx']
assert len(var) == len(
param["param_name"]
), "Length of variables != length of params"
param_dict = dict(zip(param["param_name"], var))
origin_model = model.copy()
# update model.jointPlacements
updated_params = []
# base placement or 'universe' joint
# TODO: add axis of base
if "base_placement" in list(param_dict.keys())[0]:
base_placement = cartesian_to_SE3(var[0:6])
updated_params = param["param_name"][0:6]
for j_id in param["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:
print(
"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(
(param["NbMarkers"] * param["calibration_index"], param["NbSample"])
)
# update end_effector frame
ee_frames = []
for marker_idx in range(1, param["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:
print(
"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)
ee_frames.append(eeMf)
assert (
len(ee_frames) == param["NbMarkers"]
), "Number of end-effector frames != number of markers"
# get transform
q_ = np.copy(q)
for i in range(param["NbSample"]):
pin.framesForwardKinematics(model, data, q_[i, :])
pin.updateFramePlacements(model, data)
# update model.jointPlacements with joint elastic error
if param["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 param["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, param["start_frame"], param["end_frame"]
)
# revert model back to origin from added joint elastic error
for j_id in param["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:
oMf = oMee
if param["base_to_ref_frame"] is not None:
oMf = bMo * oMf
else:
if param["calib_model"] == "joint_offset":
oMf = bMo * oMf
# final transform
trans = oMf.translation.tolist()
orient = pin.rpy.matrixToRpy(oMf.rotation).tolist()
loc = trans + orient
measure = []
for mea_id, mea in enumerate(param["measurability"]):
if mea:
measure.append(loc[mea_id])
PEE_marker[:, i] = np.array(measure)
PEE_marker = PEE_marker.flatten("C")
PEE = np.append(PEE, PEE_marker)
# revert model back to original
assert (
origin_model.jointPlacements != model.jointPlacements
), "before revert"
for j_id in param["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, param, 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 param["param_name"]
q (ndarray): Joint configurations matrix (n_samples, n_joints)
param (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 param["calib_model"] == "full_params":
axis_tpl = FULL_PARAMTPL
elif param["calib_model"] == "joint_offset":
axis_tpl = JOINT_OFFSETTPL
# order of joint in variables are arranged as in param['actJoint_idx']
assert len(var) == len(
param["param_name"]
), "Length of variables != length of params"
param_dict = dict(zip(param["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 = param["start_frame"]
end_f = param["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 param["NbMarkers"] == 1:
for marker_idx in range(1, param["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:
print(
"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 param["NbMarkers"] > 1:
print("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 param["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:
print(
"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((param["calibration_index"], param["NbSample"]))
q_ = np.copy(q)
for i in range(param["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(param["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 param["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, param):
"""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
param (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, param["IDX_TOOL"], pin.LOCAL
)
R = pin.computeFrameKinematicRegressor(
model, data, param["IDX_TOOL"], pin.LOCAL
)
return model, data, R, J
[docs]
def calculate_identifiable_kinematics_model(q, model, data, param):
"""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
param (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 = param["calibration_index"]
R = np.zeros([6 * param["NbSample"], 6 * (model.njoints - 1)])
J = np.zeros([6 * param["NbSample"], model.njoints - 1])
for i in range(param["NbSample"]):
if MIN_MODEL == 1:
q_rand = pin.randomConfiguration(model)
q_i = param["q0"]
q_i[param["config_idx"]] = q_rand[param["config_idx"]]
else:
q_i = q_temp[i, :]
if param["start_frame"] == "universe":
model, data, Ri, Ji = calculate_kinematics_model(
q_i, model, data, param
)
else:
Ri = get_rel_kinreg(
model, data, param["start_frame"], param["end_frame"], q_i
)
# Ji = np.zeros([6, model.njoints-1]) ## TODO: get_rel_jac
Ji = get_rel_jac(
model, data, param["start_frame"], param["end_frame"], q_i
)
for j, state in enumerate(param["measurability"]):
if state:
R[param["NbSample"] * j + i, :] = Ri[j, :]
J[param["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)
print(R.shape, J.shape)
if param["calib_model"] == "joint_offset":
return J
elif param["calib_model"] == "full_params":
return R
[docs]
def calculate_base_kinematics_regressor(q, model, data, param, 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
param (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 param["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_geo_offset(joint_names)
joint_offsets = get_joint_offset(model, joint_names)
# calculate kinematic regressor with random configs
if not param["free_flyer"]:
Rrand = calculate_identifiable_kinematics_model([], model, data, param)
else:
Rrand = calculate_identifiable_kinematics_model(q, model, data, param)
# calculate kinematic regressor with input configs
if np.any(np.array(q)):
R = calculate_identifiable_kinematics_model(q, model, data, param)
else:
R = Rrand
# only joint offset parameters
if param["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 param["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 param['param_name']/calibrating parameters
for j in idx_base:
param["param_name"].append(paramsrand_e[j])
print(
"shape of full regressor, reduced regressor, base regressor: ",
Rrand.shape,
Rrand_e.shape,
Rrand_b.shape,
)
return Rrand_b, R_b, R_e, paramsrand_base, paramsrand_e
[docs]
class BaseCalibration:
def __init__(self, robot, config_file, del_list=[]):
self._robot = robot
self.model = self._robot.model
self.data = self._robot.data
self.del_list_ = del_list
self.param = None
self.load_param(config_file)
self.nvars = len(self.param["param_name"])
self._data_path = abspath(self.param["data_file"])
self.STATUS = "NOT CALIBRATED"
[docs]
def initialize(self):
self.load_data_set()
self.create_param_list()
[docs]
def solve(self):
self.solve_optimisation()
self.calc_stddev()
if self.param["PLOT"]:
self.plot()
[docs]
def plot(self):
self.plot_errors_distribution()
self.plot_3d_poses()
self.plot_joint_configurations()
plt.show()
[docs]
def load_param(self, config_file, setting_type="calibration"):
with open(config_file, "r") as f:
config = yaml.load(f, Loader=SafeLoader)
calib_data = config[setting_type]
self.param = get_param_from_yaml(self._robot, calib_data)
[docs]
def create_param_list(self, q=None):
if q is None:
q_ = []
else:
q_ = q
(
Rrand_b,
R_b,
R_e,
paramsrand_base,
paramsrand_e,
) = calculate_base_kinematics_regressor(
q_, self.model, self.data, self.param, tol_qr=1e-6
)
if self.param["known_baseframe"] is False:
add_base_name(self.param)
if self.param["known_tipframe"] is False:
add_pee_name(self.param)
return True
[docs]
def load_data_set(self):
self.PEE_measured, self.q_measured = load_data(
self._data_path, self.model, self.param, self.del_list_
)
[docs]
def load_calibration_param(self, param_file):
with open(param_file, "r") as param_file:
param_dict_ = yaml.load(param_file, Loader=SafeLoader)
assert len(self.param["param_name"]) == len(
param_dict_
), "The loaded param list does not match calibration config."
self.var_ = np.zeros(len(self.param["param_name"]))
updated_var_ = []
for i_, name_ in enumerate(self.param["param_name"]):
assert name_ == list(param_dict_.keys())[i_]
self.var_[i_] = list(param_dict_.values())[i_]
updated_var_.append(name_)
assert len(updated_var_) == len(self.var_), "Not all param imported."
[docs]
def validate_model(self):
assert (
self.var_ is not None
), "Call load_calibration_param() to load model parameters first."
pee_valid_ = self.get_pose_from_measure(self.var_)
rmse_ = np.sqrt(np.mean((pee_valid_ - self.PEE_measured) ** 2))
mae_ = np.mean(np.abs(pee_valid_ - self.PEE_measured))
print("position root-mean-squared error of end-effector: ", rmse_)
print("position mean absolute error of end-effector: ", mae_)
return rmse_, mae_
[docs]
def get_pose_from_measure(self, res_):
"""
Get the pose of the robot given a set of parameters.
"""
return calc_updated_fkm(
self.model, self.data, res_, self.q_measured, self.param
)
[docs]
def calc_stddev(self):
"""
Calculate the standard deviation of the calibrated parameters.
"""
assert self.STATUS == "CALIBRATED", "Calibration not performed yet"
sigma_ro_sq = (self.LM_result.cost**2) / (
self.param["NbSample"] * self.param["calibration_index"] - self.nvars
)
J = self.LM_result.jac
C_param = sigma_ro_sq * np.linalg.pinv(np.dot(J.T, J))
std_dev = []
std_pctg = []
for i_ in range(self.nvars):
std_dev.append(np.sqrt(C_param[i_, i_]))
std_pctg.append(abs(np.sqrt(C_param[i_, i_]) / self.LM_result.x[i_]))
self.std_dev = std_dev
self.std_pctg = std_pctg
[docs]
def plot_errors_distribution(self):
"""
Plot the distribution of the errors.
"""
assert self.STATUS == "CALIBRATED", "Calibration not performed yet"
fig1, ax1 = plt.subplots(self.param["NbMarkers"], 1)
colors = ["blue", "red", "yellow", "purple"]
if self.param["NbMarkers"] == 1:
ax1.bar(np.arange(self.param["NbSample"]), self._PEE_dist[0, :])
ax1.set_xlabel("Sample", fontsize=25)
ax1.set_ylabel("Error (meter)", fontsize=30)
ax1.tick_params(axis="both", labelsize=30)
ax1.grid()
else:
for i in range(self.param["NbMarkers"]):
ax1[i].bar(
np.arange(self.param["NbSample"]),
self._PEE_dist[i, :],
color=colors[i],
)
ax1[i].set_xlabel("Sample", fontsize=25)
ax1[i].set_ylabel("Error of marker %s (meter)" % (i + 1), fontsize=25)
ax1[i].tick_params(axis="both", labelsize=30)
ax1[i].grid()
[docs]
def plot_3d_poses(self, INCLUDE_UNCALIB=False):
"""
Plot the 3D poses of the robot.
"""
assert self.STATUS == "CALIBRATED", "Calibration not performed yet"
fig2 = plt.figure()
fig2.suptitle("Visualization of estimated poses and measured pose in Cartesian")
ax2 = fig2.add_subplot(111, projection="3d")
PEEm_LM2d = self.PEE_measured.reshape(
(
self.param["NbMarkers"] * self.param["calibration_index"],
self.param["NbSample"],
)
)
PEEe_sol = self.get_pose_from_measure(self.LM_result.x)
PEEe_sol2d = PEEe_sol.reshape(
(
self.param["NbMarkers"] * self.param["calibration_index"],
self.param["NbSample"],
)
)
PEEe_uncalib = self.get_pose_from_measure(self._var_0)
PEEe_uncalib2d = PEEe_uncalib.reshape(
(
self.param["NbMarkers"] * self.param["calibration_index"],
self.param["NbSample"],
)
)
for i in range(self.param["NbMarkers"]):
ax2.scatter3D(
PEEm_LM2d[i * 3, :],
PEEm_LM2d[i * 3 + 1, :],
PEEm_LM2d[i * 3 + 2, :],
marker="^",
color="blue",
label="Measured",
)
ax2.scatter3D(
PEEe_sol2d[i * 3, :],
PEEe_sol2d[i * 3 + 1, :],
PEEe_sol2d[i * 3 + 2, :],
marker="o",
color="red",
label="Estimated",
)
if INCLUDE_UNCALIB:
ax2.scatter3D(
PEEe_uncalib2d[i * 3, :],
PEEe_uncalib2d[i * 3 + 1, :],
PEEe_uncalib2d[i * 3 + 2, :],
marker="x",
color="green",
label="Uncalibrated",
)
for j in range(self.param["NbSample"]):
ax2.plot3D(
[PEEm_LM2d[i * 3, j], PEEe_sol2d[i * 3, j]],
[PEEm_LM2d[i * 3 + 1, j], PEEe_sol2d[i * 3 + 1, j]],
[PEEm_LM2d[i * 3 + 2, j], PEEe_sol2d[i * 3 + 2, j]],
color="red",
)
if INCLUDE_UNCALIB:
ax2.plot3D(
[PEEm_LM2d[i * 3, j], PEEe_uncalib2d[i * 3, j]],
[
PEEm_LM2d[i * 3 + 1, j],
PEEe_uncalib2d[i * 3 + 1, j],
],
[
PEEm_LM2d[i * 3 + 2, j],
PEEe_uncalib2d[i * 3 + 2, j],
],
color="green",
)
ax2.set_xlabel("X - front (meter)")
ax2.set_ylabel("Y - side (meter)")
ax2.set_zlabel("Z - height (meter)")
ax2.grid()
ax2.legend()
[docs]
def plot_joint_configurations(self):
"""
Joint configurations within range bound.
"""
fig4 = plt.figure()
fig4.suptitle("Joint configurations with joint bounds")
ax4 = fig4.add_subplot(111, projection="3d")
lb = ub = []
for j in self.param["config_idx"]:
lb = np.append(lb, self.model.lowerPositionLimit[j])
ub = np.append(ub, self.model.upperPositionLimit[j])
q_actJoint = self.q_measured[:, self.param["config_idx"]]
sample_range = np.arange(self.param["NbSample"])
for i in range(len(self.param["actJoint_idx"])):
ax4.scatter3D(q_actJoint[:, i], sample_range, i)
for i in range(len(self.param["actJoint_idx"])):
ax4.plot([lb[i], ub[i]], [sample_range[0], sample_range[0]], [i, i])
ax4.set_xlabel("Angle (rad)")
ax4.set_ylabel("Sample")
ax4.set_zlabel("Joint")
ax4.grid()