129 lines
8.6 KiB
Mathematica
129 lines
8.6 KiB
Mathematica
|
function symbolicParameters = generateRobotSymbolicParameters(nbDOF, frictionIdentModel)
|
||
|
|
|
||
|
|
% Authors: Quentin Leboutet, Julien Roux, Alexandre Janot and Gordon Cheng
|
||
|
|
%
|
||
|
|
% This function generates the symbolic parameters required to describe a robot.
|
||
|
|
% These parameters will be used to build the symbolic kinematic, dynamic,
|
||
|
|
% friction and identification models of the robot.
|
||
|
|
|
||
|
|
% Allocating memory (Joint variables):
|
||
|
|
symbolicParameters.Q = sym(zeros(nbDOF,1)); % Joint position [rad]
|
||
|
|
symbolicParameters.Qp = sym(zeros(nbDOF,1)); % Joint velocity [rad/s]
|
||
|
|
symbolicParameters.Qpp = sym(zeros(nbDOF,1)); % Joint acceleration [rad/s^2]
|
||
|
|
symbolicParameters.Tau = sym(zeros(nbDOF,1)); % Joint torque [N.m]
|
||
|
|
|
||
|
|
% Allocating memory (Kinematic and Dynamic variables):
|
||
|
|
symbolicParameters.Geometry = sym(zeros(nbDOF,4)); % Position of the robot DH links [m]
|
||
|
|
symbolicParameters.GeometryCOM = sym(zeros(3,nbDOF)); % Position of the links' center of mass w.r.t the link's DH frame[m]
|
||
|
|
symbolicParameters.Moment = sym(zeros(3,nbDOF)); % First Moment
|
||
|
|
symbolicParameters.Mass = sym(zeros(nbDOF,1)); % Mass of the robot links [kg]
|
||
|
|
symbolicParameters.InertiaDH = sym(zeros(3,3,nbDOF)); % Inertia tensor around dh frame
|
||
|
|
symbolicParameters.InertiaCOM = sym(zeros(3,3,nbDOF)); % Inertia tensor around COM
|
||
|
|
symbolicParameters.Ia = sym(zeros(nbDOF,1)); % Actuator and transmission inertias
|
||
|
|
|
||
|
|
% Allocating memory (Noise variables): uncomment in case you want to generate symbolic expressions for the EKF Jacobians:
|
||
|
|
% symbolicParameters.Noise = sym(zeros(12*nbDOF,1));
|
||
|
|
% symbolicParameters.Beta = sym(zeros(12*nbDOF,1));
|
||
|
|
|
||
|
|
% Allocating memory (Friction parameters):
|
||
|
|
symbolicParameters.Z = sym(zeros(nbDOF,1)); % LuGre State
|
||
|
|
symbolicParameters.friction.Fv = sym(zeros(nbDOF,1)); % Viscous Friction
|
||
|
|
symbolicParameters.friction.Fc = sym(zeros(nbDOF,1)); % Coulomb Friction
|
||
|
|
symbolicParameters.friction.Fs = sym(zeros(nbDOF,1)); % Static Friction: only in Stribeck and LuGre models
|
||
|
|
symbolicParameters.friction.Vs = sym(zeros(nbDOF,1)); % Stribeck velocity: only in Stribeck and LuGre models
|
||
|
|
symbolicParameters.friction.Es = sym(zeros(nbDOF,1)); % Exponent: only in Stribeck and LuGre models
|
||
|
|
symbolicParameters.friction.Sigma_0 = sym(zeros(nbDOF,1)); % Contact stiffness: only in LuGre model
|
||
|
|
symbolicParameters.friction.Sigma_1 = sym(zeros(nbDOF,1)); % Damping coefficient of the bristle: only in LuGre model
|
||
|
|
symbolicParameters.friction.Sigma_2 = sym(zeros(nbDOF,1)); % Viscous friction coefficient of the bristle: only in LuGre model
|
||
|
|
symbolicParameters.friction.Z0 = sym(zeros(nbDOF,1)); % Initial deflection of the contacting asperities: only in LuGre model
|
||
|
|
symbolicParameters.friction.Tau_off = sym(zeros(nbDOF,1)); % Resulting nonlinear friction torque: only in Stribeck and LuGre models
|
||
|
|
symbolicParameters.friction.Fvm = sym(zeros(nbDOF,1)); % Coupling Viscous Friction
|
||
|
|
symbolicParameters.friction.Fcm = sym(zeros(nbDOF,1)); % Coupling Coulomb Friction
|
||
|
|
|
||
|
|
% Gravity:
|
||
|
|
gx = sym('gx','real');
|
||
|
|
gy = sym('gy','real');
|
||
|
|
gz = sym('gz','real');
|
||
|
|
symbolicParameters.Gravity = [gx;gy;gz]; % Gravity vector in world frame
|
||
|
|
|
||
|
|
% Control Period
|
||
|
|
symbolicParameters.dt = sym('dt','real');
|
||
|
|
|
||
|
|
% Symbolic parameters of a mass fixed to the end-effector (for robot drive-gains identification):
|
||
|
|
symbolicParameters.M_ef = sym('M_ef','real');
|
||
|
|
symbolicParameters.GeometryCOM_ef(1,1) = sym('X_ef','real');
|
||
|
|
symbolicParameters.GeometryCOM_ef(2,1) = sym('Y_ef','real');
|
||
|
|
symbolicParameters.GeometryCOM_ef(3,1) = sym('Z_ef','real');
|
||
|
|
symbolicParameters.Moment_ef(1,1) = sym('MX_ef','real');
|
||
|
|
symbolicParameters.Moment_ef(2,1) = sym('MY_ef','real');
|
||
|
|
symbolicParameters.Moment_ef(3,1) = sym('MZ_ef','real');
|
||
|
|
XX_ef = sym('XX_ef','real');
|
||
|
|
XY_ef = sym('XY_ef','real');
|
||
|
|
XZ_ef = sym('XZ_ef','real');
|
||
|
|
YY_ef = sym('YY_ef','real');
|
||
|
|
YZ_ef = sym('YZ_ef','real');
|
||
|
|
ZZ_ef = sym('ZZ_ef','real');
|
||
|
|
symbolicParameters.InertiaDH_ef = inertiaMatrix(XX_ef, XY_ef, XZ_ef, YY_ef, YZ_ef, ZZ_ef);
|
||
|
|
symbolicParameters.InertiaCOM_ef = inertiaMatrixDH2COM(symbolicParameters.InertiaDH_ef, symbolicParameters.M_ef, symbolicParameters.Moment_ef);
|
||
|
|
symbolicParameters.Xhi_ef = [XX_ef; XY_ef; XZ_ef; YY_ef; YZ_ef; ZZ_ef; symbolicParameters.Moment_ef; symbolicParameters.M_ef];
|
||
|
|
|
||
|
|
for i=1:nbDOF
|
||
|
|
% Joint variables:
|
||
|
|
symbolicParameters.Q(i,1) = sym(sprintf('q%d',i),'real');
|
||
|
|
symbolicParameters.Qp(i,1) = sym(sprintf('qp%d',i),'real');
|
||
|
|
symbolicParameters.Qpp(i,1) = sym(sprintf('qpp%d',i),'real');
|
||
|
|
symbolicParameters.Tau(i,1) = sym(sprintf('tau%d',i),'real');
|
||
|
|
|
||
|
|
% Robot link geometry [Beta d a alpha]:
|
||
|
|
symbolicParameters.Geometry(i,1) = sym(sprintf('theta%d',i),'real');
|
||
|
|
symbolicParameters.Geometry(i,2) = sym(sprintf('d%d',i),'real');
|
||
|
|
symbolicParameters.Geometry(i,3) = sym(sprintf('a%d',i),'real');
|
||
|
|
symbolicParameters.Geometry(i,4) = sym(sprintf('alpha%d',i),'real');
|
||
|
|
|
||
|
|
% Robot link center of mass geometry:
|
||
|
|
symbolicParameters.GeometryCOM(1,i) = sym(sprintf('X%d',i),'real');
|
||
|
|
symbolicParameters.GeometryCOM(2,i) = sym(sprintf('Y%d',i),'real');
|
||
|
|
symbolicParameters.GeometryCOM(3,i) = sym(sprintf('Z%d',i),'real');
|
||
|
|
|
||
|
|
% Actuator and transmission inertias:
|
||
|
|
symbolicParameters.Ia(i) = sym(sprintf('Ia%d',i),'real');
|
||
|
|
|
||
|
|
% Robot link masses:
|
||
|
|
symbolicParameters.Mass(i) = sym(sprintf('M%d',i),'real');
|
||
|
|
|
||
|
|
% Robot link first moment:
|
||
|
|
symbolicParameters.Moment(1,i) = sym(sprintf('MX%d',i),'real');
|
||
|
|
symbolicParameters.Moment(2,i) = sym(sprintf('MY%d',i),'real');
|
||
|
|
symbolicParameters.Moment(3,i) = sym(sprintf('MZ%d',i),'real');
|
||
|
|
|
||
|
|
% Robot link inertias (around the link DH frame):
|
||
|
|
XXi = sym(sprintf('XX%d',i),'real');
|
||
|
|
XYi = sym(sprintf('XY%d',i),'real');
|
||
|
|
XZi = sym(sprintf('XZ%d',i),'real');
|
||
|
|
YYi = sym(sprintf('YY%d',i),'real');
|
||
|
|
YZi = sym(sprintf('YZ%d',i),'real');
|
||
|
|
ZZi = sym(sprintf('ZZ%d',i),'real');
|
||
|
|
symbolicParameters.InertiaDH(:,:,i) = inertiaMatrix(XXi, XYi, XZi, YYi, YZi, ZZi);
|
||
|
|
symbolicParameters.InertiaCOM(:,:,i) = inertiaMatrixDH2COM(symbolicParameters.InertiaDH(:,:,i), symbolicParameters.Mass(i), symbolicParameters.Moment(:,i));
|
||
|
|
|
||
|
|
% Symbolic Friction parameters:
|
||
|
|
symbolicParameters.Z(i,1) = sym(sprintf('Z%d',i),'real');
|
||
|
|
symbolicParameters.friction.Fv(i,1) = sym(sprintf('Fv%d',i),'real'); % Viscous Friction
|
||
|
|
symbolicParameters.friction.Fc(i,1) = sym(sprintf('Fc%d',i),'real'); % Coulomb Friction
|
||
|
|
symbolicParameters.friction.Fs(i,1) = sym(sprintf('Fs%d',i),'real'); % Static Friction: only in Stribeck and LuGre models
|
||
|
|
symbolicParameters.friction.Vs(i,1) = sym(sprintf('Vs%d',i),'real'); % Stribeck velocity: only in Stribeck and LuGre models
|
||
|
|
symbolicParameters.friction.Es(i,1) = sym(sprintf('Es%d',i),'real'); % Exponent: only in Stribeck and LuGre models
|
||
|
|
symbolicParameters.friction.Sigma_0(i,1) = sym(sprintf('Sigma_0%d',i),'real'); % Contact stiffness: only in LuGre model
|
||
|
|
symbolicParameters.friction.Sigma_1(i,1) = sym(sprintf('Sigma_1%d',i),'real'); % Damping coefficient of the bristle: only in LuGre model
|
||
|
|
symbolicParameters.friction.Sigma_2(i,1) = sym(sprintf('Sigma_2%d',i),'real'); % Viscous friction coefficient of the bristle: only in LuGre model
|
||
|
|
symbolicParameters.friction.Z0(i,1) = sym(sprintf('Z0%d',i),'real'); % Initial deflection of the contacting asperities: only in LuGre model
|
||
|
|
symbolicParameters.friction.Tau_off(i,1) = sym(sprintf('tau_off%d',i),'real'); % Resulting nonlinear friction torque: only in Stribeck and LuGre models
|
||
|
|
symbolicParameters.friction.Fvm(i,1) = sym(sprintf('Fvm%d',i),'real'); % Couping Viscous friction parameters as described in Gautier et al. 2011.
|
||
|
|
symbolicParameters.friction.Fcm(i,1) = sym(sprintf('Fcm%d',i),'real'); % Couping Coulomb friction parameters as described in Gautier et al. 2011.
|
||
|
|
end
|
||
|
|
|
||
|
|
% Generate the vector Xhi of standard parameters:
|
||
|
|
[symbolicParameters.Xhi, ~, symbolicParameters.Xhi_aug] = getStandardParameterVector(symbolicParameters.InertiaDH, symbolicParameters.Moment, symbolicParameters.Mass, symbolicParameters.Ia, frictionIdentModel, symbolicParameters.friction);
|
||
|
|
|
||
|
|
end
|