roboskin.calibration package

Subpackages

• roboskin.calibration.utils package
  • Submodules
  • roboskin.calibration.utils.io module
  • roboskin.calibration.utils.quaternion module
  • roboskin.calibration.utils.rotational_acceleration module
  • roboskin.calibration.utils.rotational_acceleration_torch module
  • Module contents

Submodules

roboskin.calibration.convert_to_lowertriangular_matrix module

This is a python file containing a test case and a class to convert any matrix which is convertible Lower Triangular(LT) Matrix to Lower Triangular Matrix Algorithm: *) As this is a binary matrix, and also lower triangular, the total sum of elements should be at max n(n+1)/2, else it can’t be a LT matrix *) There can only be one reference segment, hence only one zero row array, else the algorithm will throw an exception *) You are only allowed to switch rows and columns to get the LT matrix Custom Sort Row:

*) Arrange the rows in ascending order of number of ones present in the row *) If there is a tie in number of ones, then you assume the row as a binary number, with MSB at first and then the number you get, convert it to a floating point number and add it to the row sum. This mainly signifies as a weight value to the row sum which are same, so that argsort can give preference. Finally the row sum is arranged in ascending order

Custom Sort Column:

*) Arrange the columns in the descending order of number of ones in the column *) *) Arrange the rows in ascending order of number of ones present in the row *) If there is a tie in number of ones, then you assume the row as a binary number, with MSB at first and then the number you get, convert it to a floating point number and add it to the row sum. This mainly signifies as a weight value to the row sum which are same, so that argsort can give preference. Finally the row sum is arranged in descending order

# TODO: Next PR, I will add more examples of why this approach is correct. I derived some in my IPad

class roboskin.calibration.convert_to_lowertriangular_matrix.ConvertToLT(input_matrix)

Bases: object

This is a class to convert LT convertible matrix to LT matrix

convert_to_floating_point(my_number)

This method is used to convert some number like 768 to 0.768. Basically used to add weight value :param my_number: Input Number :type my_number: int

Returns

floating point number from the input number

Return type

float

find_zero_ndarray(input_array)

As this is a binary matrix, with values only 0 and 1, equating min value and max value to 0, we can say the array is a zero array :param input_array: Input matrix is the matrix we want to find out whether it’s a zero array or not. :type input_array: np.ndarray

Returns

True if the array is a zero array, otherwise False

Return type

Bool

get_lt_matrix_infos()

This function is just used to return values in the order bool: Whether the matrix output generated is an LT matrix or not np.ndarray: The LT matrix, if not possible the near LT matrix. Basically whatever the output generated

from the algorithm

int: The reference segment Accelerometer ID np.ndarray: The accelerometer ID’s in the order of their arrangement on the robotic arm np.ndarray: The Joint ID’s in the order of their arrangement on the robotic arm :returns: :rtype: bool, np.ndarray, int, np.ndarray, np.ndarray

is_lower_triangular_fn(input_array)

Directly copied from: https://www.geeksforgeeks.org/program-check-matrix-lower-triangular/ And also checked. Will output if input matrix input_array is a lower triangular matrix or not :param input_array: The matrix in question, whether it’s lower triangular or not :type input_array: np.ndarray

Returns

True if numpy array inputted is lower triangular, else False

Return type

bool

is_matrix_lt_convertible(input_array)

Assuming that passed triangle is binary matrix, this will only work in that case :param input_array:

is_square_matrix(input_array)

This is a function which raises an exception, if the output matrix isn’t square matrix :param input_array: :type input_array: Input Matrix

Returns

It doesn’t return any value, but throws an exception if the passes matrix isn’t a square matrix

Return type

None

main_algorithm()

This is the main algorithm implementation. The algorithm details are at the top of the file. :returns: :rtype: None

numpyarray_toint(my_array, reverse)

This function will convert a numpy 1D array which is considered a binary number into a decimal number :param my_array: Input array :type my_array: np.ndarray :param reverse: if reverse MSB is considered first, else last :type reverse: bool

Returns

Decimal number

Return type

int

sort_columns_in_descending_order(input_array)

Sort columns of input matrix. The algorithm is specified at the top. :param input_array:

sort_rows_in_ascending_order(input_array)

Sort rows of input matrix. Algorithm at the top. References: # Stackoverflow links: https://stackoverflow.com/questions/52216526/sort-array-columns-based-upon-sum/52216674 # https://stackoverflow.com/questions/7235785/sorting-numpy-array-according-to-the-sum :param input_array: the input array we pass into the function, for it to have it’s rows sorted :type input_array: np.ndarray

Returns

The array with its rows sorted

Return type

np.ndarray

roboskin.calibration.data_logger module

class roboskin.calibration.data_logger.DataLogger(savedir, robot, method, overwrite=False)

Bases: object

add_best(i_su, **kwargs)

add_trial(global_step, imu_num, **kwargs)

end_timer(timer_name)

save()

start_timer(timer_name)

roboskin.calibration.error_functions module

class roboskin.calibration.error_functions.CombinedErrorFunction(**kwargs)

Bases: roboskin.calibration.error_functions.ErrorFunction

combined error function that allows for the error based on the cumulative sum of error functions.

initialize(data)

class roboskin.calibration.error_functions.ErrorFunction(loss)

Bases: object

Error Function class used to evaluate kinematics estimation models.

initialize(data)

class roboskin.calibration.error_functions.MaxAccelerationErrorFunction(loss, method='our')

Bases: roboskin.calibration.error_functions.ErrorFunction

Compute errors between estimated and measured max acceleration for sensor i

initialize(data)

use_max_accel_point()

conditions for update of best idx:

• the norm is greater than the current highest one.

• the time of this data lies within time_range

• the joint acceleration is also greater than the current highest one.

explanation: we use the information from both the norms of the SU acceleration and joint acceleration values. Since alpha x r, where alpha is joint acceleration is dominant in the calculation of SU acceleration, using both sources of information is more reliable and robust than just using one.

class roboskin.calibration.error_functions.StaticErrorFunction(loss)

Bases: roboskin.calibration.error_functions.ErrorFunction

Static error is an deviation of the gravity vector for p positions.

roboskin.calibration.error_functions.max_angle_func(t, i_joint, delta_t=0.0, **kwargs)

Computes current joint angle at time t joint is rotated in a sinusoidal motion during MaxAcceleration Data Collection.

Parameters

• t (int) – Current time t

• i_joint (int) –

• delta_t (float) –

roboskin.calibration.error_functions_torch module

class roboskin.calibration.error_functions_torch.MaxAccelerationErrorFunctionTorch(loss, method='normal_mittendorfer')

Bases: roboskin.calibration.error_functions.ErrorFunction

Compute errors between estimated and measured max acceleration for sensor i

class roboskin.calibration.error_functions_torch.StaticErrorFunctionTorch(loss)

Bases: roboskin.calibration.error_functions.ErrorFunction

Static error is an deviation of the gravity vector for p positions.

roboskin.calibration.error_functions_torch.max_angle_func(t)

Computes current joint angle at time t joint is rotated in a sinusoidal motion during MaxAcceleration Data Collection.

Parameters

t (int) – Current time t

roboskin.calibration.evaluator module

class roboskin.calibration.evaluator.Evaluator(true_su_pose)

Bases: object

evaluate(T, i_su)

roboskin.calibration.kinematic_chain module

class roboskin.calibration.kinematic_chain.KinematicChain(n_joint, su_joint_dict, bound_dict, linkdh_dict=None, sudh_dict=None, eval_poses=None)

Bases: object

add_temp_pose(i_joint, additional_pose)

Add a pose to the temporary pose/TM It does not initialize the temporary pose nor overwrite the current pose. The temporary pose/TM will be reset once init_temp_TM or set_poses are called.

TODO: Allow multiple additional poses

Parameters

• i_joint (int) –

• additional_pose (float) –

compute_joint_TM(i_joint, pose_type, start_joint=- 1)

Get a TransformationMatrix to the i_joint th joint

Parameters

• i_joint (int) –

• pose_type (str) –

• start_joint (int) –

Returns

Return type

TM

compute_su_TM(i_su, pose_type, start_joint=- 1)

Get a TransformationMatrix to the i_su th su

Parameters

• i_su (int) –

• pose_type (str) –

• start_joint (int) –

Returns

Return type

TM

get_params_at(i_su)

Parameters

i_su (int) – ith SU

Returns

• params (np.array) – Next DH parameters to be optimized

• bounds (np.array) – Bounds of each DH parameter

init_temp_TM(i_joint, additional_pose)

Initialize a temporary Transformation Matrices by adding an extra joint angle to Current Tranformation Matrices. Current Pose will not be updated. The temporary pose/TM will be reset once init_temp_TM or set_poses are called.

TODO: Allow multiple additional poses

Parameters

• i_joint (int) – i_joint th joint

• additional_pose (float) – Additional pose added to the pose (copied from the current pose). Current pose will not be updated.

reset_poses()

Resets current and temporary poses to 0s. Origin and Evaluation Poses will never be changed.

set_linkdh(i_joint, params)

Set i_su th SU DH Parameters

Parameters

• i_joint (int) – i_joint th joint. i_joint starts from 0 to n-1.

• params (np.ndarray) – DH Parameters of the i_joint th joint (from its previous joint)

set_params_at(i_su, params)

Set DH parameters Depending of if we are optimizing 6 (just su params) or 10 (all dh params)

Parameters

• int (i) – ith joint (sensor)

• parmas (np.array) – DH Parameters

set_poses(poses, start_joint=0, end_joint=None)

Set Current Poses.

Parameters

• poses (np.ndarray) –

• start_joint (int) –

• end_joint (int) –

set_sudh(i_su, params)

Set i_su th SU DH Parameters

Parameters

• i_su (int) – i_su th SU. i_su starts from 0 to m-1.

• params (np.ndarray) – DH Parameters of the i_su th SU (from its previous DoF)

roboskin.calibration.kinematic_chain.construct_kinematic_chain(robot_configs, imu_mappings, test_code=False, optimize_all=False, is_torch=False)

Parameters

• robot_configs (dict) –

• imu_mappings (dict) –

• test_code (bool) –

• optimize_all (bool) –

• is_torch (bool) –

roboskin.calibration.kinematic_chain_torch module

class roboskin.calibration.kinematic_chain_torch.KinematicChainTorch(n_joint, su_joint_dict, bound_dict, linkdh_dict=None, sudh_dict=None, eval_poses=None)

Bases: object

add_temp_pose(i_joint, additional_pose)

Add a pose to the temporary pose/TM It does not initialize the temporary pose nor overwrite the current pose. The temporary pose/TM will be reset once init_temp_TM or set_poses are called.

TODO: Allow multiple additional poses

Parameters

• i_joint (int) –

• additional_pose (float) –

compute_joint_TM(i_joint, pose_type, start_joint=- 1)

Get a TransformationMatrix to the i_joint th joint

Based on pose_type, computes transformation matrix to joint.

Parameters

• i_joint (int) –

• pose_type (str) –

• start_joint (int) –

Returns

Return type

TM

compute_su_TM(i_su, pose_type, start_joint=- 1)

Get a TransformationMatrix to the i_su th su

Parameters

• i_su (int) –

• pose_type (str) –

• start_joint (int) –

Returns

Return type

TM

get_params_at(i_su)

Parameters

i_su (int) – ith SU

Returns

• params (torch.Tensor) – Next DH parameters to be optimized

• bounds (torch.Tensor) – Bounds of each DH parameter

init_temp_TM(i_joint, additional_pose)

Initialize a temporary Transformation Matrices by adding an extra joint angle to Current Tranformation Matrices. Current Pose will not be updated. The temporary pose/TM will be reset once init_temp_TM or set_poses are called.

TODO: Allow multiple additional poses

Parameters

• i_joint (int) – i_joint th joint

• additional_pose (float) – Additional pose added to the pose (copied from the current pose). Current pose will not be updated.

reset_poses()

Resets current and temporary poses to 0s. Origin and Evaluation Poses will never be changed.

set_linkdh(i_joint, params)

Set i_su th SU DH Parameters

Parameters

• i_joint (int) – i_joint th joint. i_joint starts from 0 to n-1.

• params (np.ndarray) – DH Parameters of the i_joint th joint (from its previous joint)

set_params_at(i_su, params)

Set DH parameters Depending of if we are optimizing 6 (just su params) or 10 (all dh params)

Parameters

• int (i) – ith joint (sensor)

• parmas (np.array) – DH Parameters

set_poses(poses, start_joint=0, end_joint=None)

sets poses from start_joint to end_joint.

Parameters

• poses (np.ndarray) –

• start_joint (int) –

• end_joint (int) –

set_sudh(i_su, params)

Set i_su th SU DH Parameters

Parameters

• i_su (int) – i_su th SU. i_su starts from 0 to m-1.

• params (np.ndarray) – DH Parameters of the i_su th SU (from its previous DoF)

roboskin.calibration.kinematic_chain_torch.construct_kinematic_chain(robot_configs, imu_mappings, test_code=False, optimize_all=False)

Parameters

• robot_configs (dict) –

• imu_mappings (dict) –

• test_code (bool) –

• optimize_all (bool) –

roboskin.calibration.loss module

class roboskin.calibration.loss.L1Loss

Bases: roboskin.calibration.loss.Loss

L1 Loss

class roboskin.calibration.loss.L1LossTorch

Bases: roboskin.calibration.loss.Loss

pytorch’s l1loss for tensors.

class roboskin.calibration.loss.L2Loss

Bases: roboskin.calibration.loss.Loss

L2Loss.

class roboskin.calibration.loss.L2LossTorch

Bases: roboskin.calibration.loss.Loss

pytorch’s l2loss for tensors.

class roboskin.calibration.loss.Loss

Bases: object

Loss function base class

class roboskin.calibration.loss.MeanAbsoluteLoss

Bases: roboskin.calibration.loss.Loss

mean absolute loss

class roboskin.calibration.loss.MeanSquareLoss

Bases: roboskin.calibration.loss.Loss

mean square loss

class roboskin.calibration.loss.SmoothL1LossTorch

Bases: roboskin.calibration.loss.Loss

pytorch’s smooth l1loss for tensors.

roboskin.calibration.optimizer module

class roboskin.calibration.optimizer.IncrementalOptimizerBase(kinematic_chain, evaluator, data_logger, optimize_all, error_functions, stop_conditions)

Bases: roboskin.calibration.optimizer.OptimizerBase

optimize(data)

Optimize SU from Base to the End-Effector incrementally

class roboskin.calibration.optimizer.MittendorferMethodOptimizer(kinematic_chain, evaluator, data_logger, optimize_all, error_function_=None, stop_condition_=None, method='mittendorfer')

Bases: roboskin.calibration.optimizer.MixedIncrementalOptimizer

class roboskin.calibration.optimizer.MixedIncrementalOptimizer(kinematic_chain, evaluator, data_logger, optimize_all, error_function, stop_condition)

Bases: roboskin.calibration.optimizer.IncrementalOptimizerBase

objective(params, grad)

Objective function used in the optimizer. Called continuouly until a stopping condition is met.

Parameters

• params – Predicted DH Parameters

• grad – Gradient

class roboskin.calibration.optimizer.OptimizerBase(kinematic_chain, evaluator, data_logger, optimize_all)

Bases: object

TODO: All Optimizer will inherit this class

optimize()

Optimize, evaluate and log

property result

Return dict - average_errors of all SU

• euclidean_distance

• quaternion_distance

• best

  • errors for all SU

    • euclidean_distance

    • quaternion_distance

  • params for all SU

  • positions for all SU

  • orientations for all Su

• trials

  • params

  • pose

  • positions

  • orientations

class roboskin.calibration.optimizer.OurMethodOptimizer(kinematic_chain, evaluator, data_logger, optimize_all, error_functions_=None, stop_conditions_=None, method='our')

Bases: roboskin.calibration.optimizer.SeparateIncrementalOptimizer

class roboskin.calibration.optimizer.SeparateIncrementalOptimizer(kinematic_chain, evaluator, data_logger, optimize_all, error_functions, stop_conditions)

Bases: roboskin.calibration.optimizer.IncrementalOptimizerBase

objective(target_params, grad)

Computes an error e_T = e_1 + e_2 from current parameters

Parameters

• target_params (list of floats) – Target parameters to be estimated

• grad (np.ndarray) – Gradient, but we do not use any gradient information (We could in the future)

Returns

error – Error between measured values and estimated model outputs

Return type

float

class roboskin.calibration.optimizer.TorchOptimizerBase(kinematic_chain, evaluator, data_logger, optimize_all, error_function_=None, stop_condition_=None, method='our')

Bases: roboskin.calibration.optimizer.IncrementalOptimizerBase

torch optimizer. This optimizer is the only optimizer that takes into account gradients during runtime. This is done through the .backward() function of PyTorch tensors (with is_grad set to True), which will automatically calculate the gradients of the parameters (DH parameters) based on the error.

get_gradients(params)

gets the gradients of each dh parameter (which are set to params).

nlopt_objective(params, grad)

objective function with gradients already having been calculated.

optimize(data)

Pytorch optimization. Simply calculates the gradients.

vanilla_optimizer(params, bounds)

uses pytorch for vanilla SGD in order to optimize the DH parameters.

roboskin.calibration.optimizer.choose_optimizer(args, kinematic_chain, evaluator, data_logger, optimize_all)

roboskin.calibration.stop_conditions module

class roboskin.calibration.stop_conditions.CombinedStopCondition(**kwargs)

Bases: roboskin.calibration.stop_conditions.StopCondition

Stop condition base class

initialize()

update(x, y, e)

return e, x

class roboskin.calibration.stop_conditions.DeltaXStopCondition(windowsize=10, threshold=0.001, retval=1e-05)

Bases: roboskin.calibration.stop_conditions.StopCondition

DeltaXStopCondition class. Keeps track on the differences in x from iteration to iteration, until the updates are very small.

initialize()

update(x, y, e)

return e, x

class roboskin.calibration.stop_conditions.MaxCountStopCondition(count_limit=1000)

Bases: roboskin.calibration.stop_conditions.StopCondition

Stop condition base class

initialize()

update(x, y, e)

return e, x

class roboskin.calibration.stop_conditions.PassThroughStopCondition

Bases: roboskin.calibration.stop_conditions.StopCondition

PassThroughStopCondition class.

update(x, y, e)

return e, x

class roboskin.calibration.stop_conditions.StopCondition

Bases: object

Stop condition base class

initialize()

update(x, y, e)

return e, x

roboskin.calibration.transformation_matrix module

class roboskin.calibration.transformation_matrix.TransformationMatrix(theta=None, d=None, a=None, alpha=None)

Bases: object

Class for Transformation Matrix Manages all its parameters and computation It also outputs Rotation Matrix and Position of the transformed result

property R

Rotation Matrix

Returns

Rotation Matrix

Return type

np.ndarray

copy_fnc(input)

generic copy function.

classmethod from_bounds(bounds, keys=['theta', 'd', 'a', 'alpha'])

classmethod from_dict(params_dict)

classmethod from_list(params, keys=['theta', 'd', 'a', 'alpha'])

classmethod from_numpy(params, keys=['theta', 'd', 'a', 'alpha'])

mm_fnc(x1, x2)

depending on if the matrix was converted to a tensor, determine the correct matrix multiplication function.

numpy()

converts transformation matrix to np array, and returns the class.

numpy_()

converts transformation matrix to a numpy array in place.

property parameters

returns: DH parameters of this transformation matrix :rtype: np.array

property position

Position as a result of the transformation

Returns

Position of the resulting transformation

Return type

np.ndarray

property quaternion

Quaternion as a result of the transformation

set_params(params, keys=['theta', 'd', 'a', 'alpha'])

Set parameters that have been optimized

Parameters

params (np.array) – DH parameters

tensor()

converts transformation matrix to tensor, and returns the class.

tensor_()

converts transformation matrix to a tensor in place.

transformation_matrix(th, d, a, al)

Create a transformation matrix DH Parameters are defined with only 4 parameters. 2 Translational parameters and 2 Rotations parameters. Here, we follow the “Modified DH Parameter” notation and not the original classic DH Parameter invnted by Denavit and Hartenberg.

From (n-1)th coordinate frame, 1. Rotate for al [rad] around x axis (Rx). 2. Displace for a [m] along x axis (Tx). 3. Rotate for th [rad] around z axis (Rz) 4. Displace for d [m] along z axis (Tz) to get to the nth coordinate frame in this order.

..math:: {}^{n-1}_{n}T = Rx * Tx * Rz * Tz ..math:

\left[
egin{array}{c|c}
    {}^{n-1}_{n} R & {}^{n-1}_{n}P \
    \hline
    0 & 1
\end{array}

ight]

The superscript represents which frame the variable is in, and the subscript represents from which frame the variable is stated. For example, \({}^{0}_{1}P\) represents the position of the 1st link (frame) in the the world frame 0. So if you want to compute the end-effector’s position in the world frame, you write as \({}^{0}_{6}P\).

If you want to rotate the gravity vector from world (frame 0) to SU 6, you write as \({}^{SU_6}g = {}^{SU_6}_{0}R * {}^{0}g\). You can compute \({}^{SU_6}_{0}R\) by,

..math::

{}^{0}_{SU_6}T = {}^{0}_{1}T * {}^{1}_{2}T * … * {}^{6}_{SU_6}T {}^{SU_6}_{0}R = ({}^{0}_{SU_6}T).R.T

Note that

Rz * Tz = Tz * Rz Rx * Tx = Tx * Rx

Source: - http://www4.cs.umanitoba.ca/~jacky/Robotics/Papers/spong_kinematics.pdf - https://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters#Modified_DH_parameters

th:

Rotation theta around z axis (rad)

d:

Displacement relative to z axis (m)

a:

Displacement relative to x axis (m)

al:

Rotation alpha around x axis (rad)

np.ndarray

transformation matrix returns 4x4 matrix of the form

Module contents