pyDynaMapp.dynamics package

Submodules

pyDynaMapp.dynamics.regressor module

class pyDynaMapp.dynamics.regressor.Regressor(robot: Robot = None)

Bases: object

addActuatorInertia(q, v, a, param)

addFriction(q, v, a, param)

addJointOffset(q, v, a, param)

computeBasicRegressor(q: numpy.ndarray = None, v: numpy.ndarray = None, a: numpy.ndarray = None)

computeBasicSparseRegressor(q, v, a)

the torque of joint i do not depend on the torque of joint i-1

computeDifferentialRegressor(q, v, a, x, dx=0.01)

This function differentiates the computeIdentificationModel of the class robot. Assuming the model is not linear with respect to parameter vector x:

τ = f(q, qp, qpp, x)

Args:

• q: ndarray, joint positions

• v: ndarray, joint velocities

• a: ndarray, joint accelerations

• dx: float, small perturbation for finite difference

Returns:

• W: ndarray, (NSamples*ndof, NParams) regressor matrix

computeDifferentiationError(q, v, a, x, dx=0.001)

retun the gradient differentiation error

computeFullRegressor(q: numpy.ndarray = None, v: numpy.ndarray = None, a: numpy.ndarray = None)

Compute the Regressor matrix of the robot This function builds the basic regressor of the 10(+4) parameters 'Ixx','Ixy','Ixz','Iyy','Iyz','Izz','mx','my','mz','m'+ ('ia','fs','fv')

Args:

• q: (ndarray) a configuration position vector

• v: (ndarray) a configuration velocity vector

• a: (ndarray) a configutation acceleration vector

Returns:

• W_mod: (ndarray) basic regressor for 10(+4) parameters

  ( NSamples * ndof, ( 10 + add_col ) * ndof)

computeReducedRegressor(q, v, a, tol=1e-06)

Eliminates columns which has L2 norm smaller than tolerance. Args:

• W: (ndarray) joint torque regressor

• tol_e: (float) tolerance

Returns:

• Wred: (ndarray) reduced regressor

computeRegressionCriterion(torque, q, v, a, x) → float

Compute the Regression error model : ε = τ - W.Θ

pyDynaMapp.dynamics.robot module

class pyDynaMapp.dynamics.robot.Robot(urdf_file_path, config_file_path, q=None, v=None, a=None)

Bases: object

Base class for Robot models.

Parameters:

urdf_file_path (str): Manipulator URDF file path. config_file_path (str): Manipulator configuration file path. model (Pinocchio.Model): Pinocchio multibody model of the manipulator. params (dict): Model static parameters. data (Pinocchio.Data): Pinocchio multibody data model. q (np.ndarray): Joints position vector. v (np.ndarray): Joints velocity vector. a (np.ndarray): Joints acceleration vector.

computeActuatorTorques(q, qp, qpp)

Estimates the joints motors torque from position, velocity and acceleration. Args:

• q: Joints position (Nsamples * ndof)

• qp: Joints velocity (Nsamples * ndof)

Returns:

• tau_m : numpy.ndarry (Nsamples *ndof)

computeBaseInertiasParams()

#TODO to implement Compute the manipulator Base inertial parameters Returns

base_params : numpy-ndarry

computeCorlolisMatrix(qp=None, q=None, timeStep=0.001, scaling=0.01)

Compute the corlolis matrix using finite difference method. Args:

• qp: Joints Velocity vector.

• q : Joints Position vector.

Returns:

• C : numpy.ndarray

# TODO ensure that the divion in the in derivates are not by a zero elmenent

computeDifferentialModel(q=None, qp=None, qpp=None, inertia_params=None)

computeFrictionTorques(qp: numpy.ndarray, q: numpy.ndarray)

Estimates the friction torque vector in robot joints given a constant joint velocity vector.

Args:

• qp : Joints velocity vector ( numSamples * ndof )

• q : Joints position vector ( numSamples * ndof )

• tspan : Simulation time duration (seconds)

• sampling : Sampling frequency (Hz)

Returns:

tau_f : Joints friction torques ( numSamples * ndof )

computeGeneralizedTorques(q=None, qp=None, qpp=None, fext=None)

Compute the genralized Torques using the recursive netwon euler alogrithm.

Args:

• q : Joints position vector. ( nq * 1 )

• qp : Joints velocity vector. ( nq * 1 )

• qpp : Joints acceleration vector. ( nq * 1 )

• fext : Joints external forces vector. ( nq * 1 )

computeGravityTorques(q=None)

Computes the joints gravity torques. Args:

• q : Joints poistion vector.

Returns:

• tau_g : numpy.ndarray.

computeIdentificationModel(x: numpy.ndarray)

This function require the setup up of the joints tarjectory parmters previlouslly ie self.q, v and a should be puted in the trajectoy or it will use the default. initlize the robot structure with the trajectory data from begin. Args:

xparamters: [inertia, friction, stiffness, actuator, fext]

[13n, 5n, n, 10n, 6]

Returns:

tau : model output torque (Nsamples * n )

computeMassMatrix(q=None, scaling=0.001)

Compute the mass matrix of the robot. Args:

• q : Joints position vector.

• scaling : Float used in scaling the mass matrix to prevent singulaty

computeStiffnessTorques(q=None)

Compute the torques resulting from joints torsional stiffness eliminated from actuator torque.

Args:

q (numpy.ndarray): Configuration vector.

Returns:

numpy.ndarray: Joints stiffness torques.

computeTrajectoryTorques(q, qp, qpp, fext=None)

Compute the joint torques given the trajectory data: position, velocity, and accelerations with fext = 0 (no external forces) and no friction.

Args:

q - (numpy ndarray) Joint positions (NSamples, ndof) qp - (numpy ndarray) Joint velocities (NSamples, ndof) qpp - (numpy ndarray) Joint accelerations (NSamples, ndof) fext - (numpy ndarray) Joints external angular torques applied (Nsamples * ndof)

Returns:

tau - numpy ndarray Joint torques (NSamples, ndof, 6) (3D force + 3D torque)

genralizedInertiasParams()

Returns the genralized inertia paramters vector Returns:

• inertia_vectors (numpy.ndarry)concatenated inertia paramters of all

  links

getActuatorInertiasMatrix() → numpy.ndarray

getStiffnessMatrix() → numpy.ndarray

Return the diagonal stiffness matrix of the robot formed by each joint stiffness factor.

setIdentificationModelData(q, qp, qpp) → None

setRandomInertiaParams() → None

set the robot links inertia paramters to random values.

updateActuatorParams(new_params: numpy.ndarray) → None

Updates the joints actuator parameters. Bounds for torque and current (Tmax, Imax) are exclued from update.

Args:

new_params [kt, inertia, damping, Ta, Tb, Tck] (numpy-ndarry) 1 * 10.ndof

updateExternalForces(F: numpy.ndarray) → None

update the Pinnchoi external forces vector.

updateFrictionParams(new_params) → None

update the robot friction parameters. Args:

• new_params : ndarry of size min 14 (2n) max is 35(5n)

updateInertiaParams(inertia_vector) → None

Update the inertia paramters vector

updateStiffnessParams(new_params) → None

Update the joints stiffness paramters. Args:

new_params (numpy ndarry)

pyDynaMapp.dynamics.state_space module

class pyDynaMapp.dynamics.state_space.StateSpace(urdf_file_path, config_file_path)

Bases: object

Base class for state space identification models

Args:

robot - Manipulator Base model

computeAugmentedStateMatrices(q_or_x: numpy.ndarray, qp: numpy.ndarray = None)

Computes and returns the augmented state-space matrices for the transformed state vector z = [x, u].

Args:

q_or_x (np.ndarray): State vector q or x. qp (np.ndarray): State derivative vector (q_dot or x_dot).

Returns:

tuple: Augmented state-space matrices (A_aug, B_aug, C_aug, D_aug).

computeCtlbMatrix(q_or_x: numpy.ndarray, qp: numpy.ndarray = None)

Compute the controllability matrix of the robot

computeGaussianNoise(length)

computeObsMatrix(q_or_x: numpy.ndarray, qp: numpy.ndarray = None)

Compute the observaliblite matrix of the robot

computePoissonNoise(length)

computeReducedStateMatrices(q: numpy.ndarray, qp: numpy.ndarray, tau: numpy.ndarray)

computes the new transformed system by diagonalized the state matrix A

computeStateEigvals(q_or_x: numpy.ndarray, qp: numpy.ndarray = None)

Returns the dynamics system eigvalues

computeStateInputVector(states: numpy.ndarray, input_torque: numpy.ndarray = None)

Computes the state space input torques vector U. the input at the time date "i" require data of i-1,i-2,...,1 states vectors Returns:

• U numpy.ndarry ( ndof * NSamples )

computeStateMatrices(q_or_x: numpy.ndarray, qp: numpy.ndarray = None)

Compute the state space matrices of the robot model.

Args:

• q_or_x: Joints position vector q or system state vector x

• qp: Joints velocity vector (required if q_or_x is q)

Returns:

• A, B, C, D matrices

computeStateTransitionMatrix(tf, ti=0)

compute the state transition matrix of the system Args:

• tf : final integartion time

• ti: initial intergartion time

getAugmentedStateVector(q: numpy.ndarray, qp: numpy.ndarray, tau: numpy.ndarray)

Compute the Augemnted state vector

getStateVector(qp: numpy.ndarray, q: numpy.ndarray)

Compute the State vector give the joints poistion and velocity. Returns:

x - numpy-ndaryy (2.ndof * 1)

linearize(states_matrix: numpy.ndarray)

Computes a- equivalent LTI system by avaraging the state depend Linear system. Args:

states_matrix : (2*ndof, NSamples)

lsim(x0: numpy.ndarray, input: numpy.ndarray, system_poles=None, noise='gaussian', verbose: bool = False)

Simulate the system response with a given input torque. Args:

• x0 : initial system state. ( 2.ndof * 1 )

• input : Input torque to the system (NSamples * ndof )

• noise : additive state noise model

• verbose: logging display the current iteration

Return:

• states : numpy-ndarry stroing iteration states vectors. ( 2.ndof * NSamples)

stabilize(A, B, desired_poles: numpy.ndarray = None)

Check what ever the numerical recursive control scheme given by :

x(k+1) = Ax(k) + B u

is stable or not and adjust it if necessary within ploes placement strategy.

state_place_poles(gain_matrix: numpy.ndarray, states: numpy.ndarray, verbose: bool = False)

this method is extention of scipy.signals.place_poles for state depend varing systems, over each sample data

x_{k+1} = A(k)x_{k} + B(k)u_{k}

Args:

• gain_matrix :

Returns:

AA : numpy.ndarrry [A(1), A(2), ... A(N)] ( 2*ndof , 2*ndof*N)

updateStateVector(q_or_x: numpy.ndarray, qp_or_tau: numpy.ndarray, tau: numpy.ndarray = None, state_poles=None)

Compute the discrete state vector at time date t + 1 given position and velocity or system state vector x, and torques vector at time date t.

visualizeRootLocus(q_or_x: numpy.ndarray, qp: numpy.ndarray = None) → None

Plot the system root locus for a given trajectory.

visualizeStatePoles(q, qp)

Plot the system poles for a given trajectory

Module contents