Tools

Robot

class figaroh.tools.robot.Robot(robot_urdf, package_dirs, isFext=False, freeflyer_ori=None)[source]

Bases: RobotWrapper

Robot class extending Pinocchio’s RobotWrapper with additional features.

display_q0()[source]

Display robot in initial configuration.

Uses either Gepetto (-g) or Meshcat (-m) visualizer based on command line argument.

get_standard_parameters(param)[source]

Get standard inertial parameters from URDF model.

Parameters:: param – Dictionary of parameter settings
Returns:: Parameter names mapped to values
Return type:: dict

Robot Collisions

class figaroh.tools.robotcollisions.CollisionWrapper(robot, geom_model=None, geom_data=None, viz=None)[source]

Bases: object

Wrapper class for handling collision checking and visualization.

add_collisions()[source]

check_collision(q)[source]

computeCollisions(q, geom_data=None)[source]

createDisplayPatchs(ncollisions)[source]

displayCollisions(collisions=None)[source]

Display collision contacts in visualization.

Parameters:: collisions – Optional list of collision triplets

displayContact(ipatch, contact)[source]

Display contact indicator in visualization.

Parameters:

ipatch – Index for naming displayed contact
contact – Contact object from collision results

getAllpairs()[source]

getCollisionDistances(collisions=None)[source]

Get minimum distances for collision pairs.

Parameters:: collisions – Optional list of collision triplets
Returns:: Array of minimum distances
Return type:: ndarray

getCollisionList()[source]

Get list of collision triplets.

Returns:

Triplets [index, collision, result] where:

index: Index of collision pair
collision: gmodel.collisionPairs[index]
result: gdata.collisionResults[index]

Return type:

list

getDistances()[source]

initDisplay(viz=None)[source]

remove_collisions(srdf_model_path)[source]

Robot Visualization

figaroh.tools.robotvisualization.display_COM(model: Model, data: Data, viz, q: ndarray, IDX: list) → None[source]

Display center of mass positions for each link.

Parameters:

model – Pinocchio robot model
data – Pinocchio robot data
viz – Robot visualizer
q – Joint configuration vector
IDX – List of frame indices in kinematic tree order

figaroh.tools.robotvisualization.display_axes(model: Model, data: Data, viz, q: ndarray) → None[source]

Display coordinate axes for each joint.

Parameters:

model – Pinocchio robot model
data – Pinocchio robot data
viz – Robot visualizer
q – Joint configuration vector

figaroh.tools.robotvisualization.display_bounding_boxes(viz, model: Model, data: Data, q: ndarray, COM_min: ndarray, COM_max: ndarray, IDX: list) → None[source]

Display COM bounding boxes for optimization.

Parameters:

viz – Robot visualizer
model – Pinocchio robot model
data – Pinocchio robot data
q – Joint configuration vector
COM_min – Min COM boundaries per segment (3*num_segments)
COM_max – Max COM boundaries per segment (3*num_segments)
IDX – List of frame indices

figaroh.tools.robotvisualization.display_force(viz, phi: Force, M_se3: SE3) → None[source]

Display force vector in visualization.

Parameters:

viz – Robot visualizer
phi – 6D force vector in M_se3 frame
M_se3 – SE3 transformation for force display

figaroh.tools.robotvisualization.display_joints(viz, model: Model, data: Data, q: ndarray) → None[source]

Display joint frames in visualization.

Parameters:

viz – Robot visualizer
model – Pinocchio robot model
data – Pinocchio robot data
q – Joint configuration vector

figaroh.tools.robotvisualization.place(viz, name: str, M: SE3) → None[source]

Place coordinate system in GUI visualization.

Parameters:

viz – Robot visualizer (e.g. gepetto-viewer)
name – Name of coordinate system object
M – Homogeneous transformation matrix

figaroh.tools.robotvisualization.rotation_matrix_from_vectors(vec1: ndarray, vec2: ndarray) → ndarray[source]

Find rotation matrix aligning vec1 to vec2.

Parameters:

vec1 – Source 3D vector
vec2 – Destination 3D vector

Returns:

3x3 rotation matrix that aligns vec1 with vec2

Return type:

ndarray

Regressor

figaroh.tools.regressor.add_coupling_TX40(W, model, data, N, nq, nv, njoints, q, v, a)[source]

Dedicated function for Staubli TX40.

Parameters:

W – Input regressor matrix
model – Robot model
data – Robot data
N – Number of samples
nq – Number of positions
nv – Number of velocities
njoints – Number of joints
q – Joint positions
v – Joint velocities
a – Joint accelerations

Returns:

Updated regressor matrix

Return type:

ndarray

figaroh.tools.regressor.build_regressor_basic(robot, q, v, a, param, tau=None)[source]

Build basic regressor for dynamic parameters.

Parameters:

robot – Robot model object
q – Configuration position array
v – Configuration velocity array
a – Configuration acceleration array
param – Dictionary of options controlling which parameters to include
tau – Optional torque measurements array for torque offsets

Returns:

Basic regressor matrix for standard parameters

Return type:

ndarray

figaroh.tools.regressor.build_regressor_reduced(W, idx_e)[source]

Build reduced regressor matrix.

Parameters:

W – Input regressor matrix
idx_e – Indices of columns to eliminate

Returns:

Reduced regressor matrix

Return type:

ndarray

figaroh.tools.regressor.build_total_regressor_current(W_b_u, W_b_l, W_l, I_u, I_l, param_standard_l, param)[source]

Build regressor for total least squares with current measurements.

Parameters:

W_b_u – Base regressor for unloaded case
W_b_l – Base regressor for loaded case
W_l – Full regressor for loaded case
I_u – Joint currents in unloaded case
I_l – Joint currents in loaded case
param_standard_l – Standard parameters in loaded case
param – Dictionary of settings

Returns:

Total regressor matrix
Normalized parameter vector
Residual vector

Return type:

tuple

figaroh.tools.regressor.build_total_regressor_wrench(W_b_u, W_b_l, W_l, tau_u, tau_l, param_standard_l, param)[source]

Build regressor for total least squares with external wrench measurements.

Parameters:

W_b_u – Base regressor for unloaded case
W_b_l – Base regressor for loaded case
W_l – Full regressor for loaded case
tau_u – External wrench in unloaded case
tau_l – External wrench in loaded case
param_standard_l – Standard parameters in loaded case
param – Dictionary of settings

Returns:

Total regressor matrix
Normalized parameter vector
Residual vector

Return type:

tuple

figaroh.tools.regressor.eliminate_non_dynaffect(W, params_std, tol_e=1e-06)[source]

Eliminate columns with L2 norm smaller than tolerance.

Parameters:

W – Joint torque regressor matrix
params_std – Standard parameters dictionary
tol_e – Tolerance value

Returns:

Reduced regressor matrix
List of parameters corresponding to reduced regressor columns

Return type:

tuple

figaroh.tools.regressor.get_index_eliminate(W, params_std, tol_e=1e-06)[source]

Get indices of columns to eliminate based on tolerance.

Parameters:

W – Joint torque regressor matrix
params_std – Standard parameters dictionary
tol_e – Tolerance value

Returns:

List of indices to eliminate
List of remaining parameters

Return type:

tuple

QR Decomposition

figaroh.tools.qrdecomposition.QR_pivoting(tau: ndarray, W_e: ndarray, params_r: list, tol_qr: float = 1e-08) → tuple[source]

Calculate QR decomposition with pivoting and find base parameters.

Parameters:

tau – Measurement vector
W_e – Regressor matrix after eliminating zero columns
params_r – List of parameters corresponding to W_e
tol_qr – Tolerance for rank determination

Returns:

(W_b, base_parameters) containing:

W_b: Base regressor matrix
base_parameters: Dictionary mapping parameter names to values

Return type:

tuple

figaroh.tools.qrdecomposition.build_baseRegressor(W_e: ndarray, idx_base: tuple) → ndarray[source]

Create base regressor matrix.

Parameters:

W_e – Original regressor matrix
idx_base – Indices of base parameters

Returns:

Base regressor matrix

Return type:

ndarray

figaroh.tools.qrdecomposition.cond_num(W_b: ndarray, norm_type: str = None) → float[source]

Calculate condition number of a matrix.

Parameters:

W_b – Input matrix
norm_type – Type of norm to use (‘fro’ or ‘max_over_min_sigma’)

Returns:

Condition number

Return type:

float

figaroh.tools.qrdecomposition.double_QR(tau: ndarray, W_e: ndarray, params_r: list, params_std: dict = None, tol_qr: float = 1e-08) → tuple[source]

Perform double QR decomposition to find base parameters.

Parameters:

tau – Measurement vector
W_e – Regressor matrix after eliminating zero columns
params_r – List of parameters corresponding to W_e
params_std – Standard parameters dictionary (optional)
tol_qr – Tolerance for rank determination

Returns:

Contains combinations of:

W_b: Base regressor matrix
base_parameters: Dictionary of base parameters
params_base: List of base parameter names
phi_b: Base parameter values
phi_std: Standard parameter values (if params_std provided)

Return type:

tuple

figaroh.tools.qrdecomposition.get_baseIndex(W_e: ndarray, params_r: list, tol_qr: float = 1e-08) → tuple[source]

Find linearly independent parameters.

Parameters:

W_e – Regressor matrix
params_r – Parameter dictionary
tol_qr – Tolerance for rank determination

Returns:

Indices of independent parameters

Return type:

tuple

figaroh.tools.qrdecomposition.get_baseParams(W_e: ndarray, params_r: list, params_std: dict = None, tol_qr: float = 1e-08) → tuple[source]

Get base parameters and regressor matrix.

Parameters:

W_e – Regressor matrix
params_r – List of parameters
params_std – Standard parameters (optional)
tol_qr – Tolerance for rank determination

Returns:

(W_b, params_base, idx_base) containing:

W_b: Base regressor matrix
params_base: List of base parameter expressions
idx_base: Indices of independent parameters

Return type:

tuple