From 11280b9783b85444c34bfd020b2ff2d51d1a84ae Mon Sep 17 00:00:00 2001 From: shamil Date: Sat, 10 Apr 2021 11:11:18 +0300 Subject: [PATCH] modifying readme 2 --- README.md | 25 +++++++++++++------------ trajectory_optmzn/fourier_series_traj.m | 16 ++++++++-------- trajectory_optmzn/mixed_traj.m | 6 +++--- ur_base_params_QRlgr.m | 13 +++++++++++++ 4 files changed, 37 insertions(+), 23 deletions(-) diff --git a/README.md b/README.md index 9c8628d..a9108dd 100644 --- a/README.md +++ b/README.md @@ -1,19 +1,20 @@ -<<<<<<< Updated upstream -# Here inertial parameter identification of robotic systems is implemented. -# Identification consistes of several steps: -# -> finding base parameters -# -> trajectory optimization -# -> running the trajectory on real robot and recording obtained trajectories -# -> offline data processing -# -> parameter estimation -======= # Dynamic calibration (dynamic parameter indentification) for rigid body manipulator The code was developed in the framework of the human-robot interaction project at [Innopolis University](https://innopolis.university/en/). One of the outputs of the project was a paper -- [Practical Aspects of Model-Based Collision Detection](https://www.frontiersin.org/articles/10.3389/frobt.2020.571574/full), where we provide some review of the recent developments in the field of dynamic calibration, outline the steps required for dynamic parameter identification and provide many useful references. If you have questions from theoretical perspective, please check the paper first. If find paper and code useful, consider citing it in your own papers. The parameter identification prcedure can be divided into several steps: ## Finding regressor Right now, there are two ways to find regressor matrix: -1. using screw algorithm (function *screw_regressor.m* or *screw_regressor2.m*) -2. using generated matrix function. The expression for the regressor is obtained symbolically using Euler-Lagrage equations, and then the function is generated from symbolic expression (script *ur_regressors_lgr.m*) +1. using screw algorithm: functions *screw_regressor.m* or *screw_regressor2.m*) +2. using generated matrix function. The expression for the regressor is obtained symbolically using Euler-Lagrage equations, and then the function is generated from symbolic expression: script *ur_regressors_lgr.m*. + Keep in mind that two methods output different regressors, the reason for that is the way you structure the vector of standard parameters! ->>>>>>> Stashed changes + +## Finding base parameters +Some standard parameters do not affect the dynamics of the robot (for example, majority of the dynamic parameters of the first link), and some enter the dynamics in linear combination with other parameters. Because of that the regressor matrix is not full rank, and is not invertible - the property we need for least squares estimation. We can find so-called base parameters of the robot, such that corresponding regressor matrix is full rank. You can find two methods for finding base parameters: +1. numerical method that uses QR decomposition: scripts *ur_base_params_QRlgr.m* and *ur_base_params_QRscrw.m*. Because I was mostly using *ur_base_params_QRlgr.m* it is better documented. +2. symbolic method: function *khalil_gautier.m*) + +I encourage you to use numerical method, as it is easy to use and understand. Moreover, when symbolic method works only with inertial parameters of the link. + +## Trajectory optimization +To get accurate estimates of the dynamic parameters, we need to make sure that the data we use has the property called persistent excitation. To achieve that it is necessary to perform trajectory optimization, in other words experiment design. As a trajectory I use the combination of truncated Fourier series (*fourier_series_traj.m*) and fifth order polynomial, yielding a function *mixed_traj.m*. As an objective function of the optimization problem, the condition number of the observation matrix (aggregated base regressor functions) is used (see *traj_cost_lgr.m* or *traj_cost_scrw.m*). diff --git a/trajectory_optmzn/fourier_series_traj.m b/trajectory_optmzn/fourier_series_traj.m index e06a8bc..c4c08d4 100644 --- a/trajectory_optmzn/fourier_series_traj.m +++ b/trajectory_optmzn/fourier_series_traj.m @@ -13,11 +13,11 @@ function [q,qd,q2d] = fourier_series_traj(t,q0,A,B,w,N) % qd - velocities i.e. derivative of polynomail % q2d - accelerations i.e. second derivative of polynomial % ----------------------------------------------------------------------- - q = q0; - qd = zeros(size(q0)); - q2d = zeros(size(q0)); - for k = 1:N - q = q + A(:,k)/(w*k).*sin(w*k*t) - B(:,k)/(w*k).*cos(w*k*t); - qd = qd + A(:,k).*cos(w*k*t) + B(:,k)*sin(w*k*t); - q2d = q2d - A(:,k)*w*k.*sin(w*k*t) + B(:,k)*w*k*cos(w*k*t); - end +q = q0; +qd = zeros(size(q0)); +q2d = zeros(size(q0)); +for k = 1:N + q = q + A(:,k)/(w*k).*sin(w*k*t) - B(:,k)/(w*k).*cos(w*k*t); + qd = qd + A(:,k).*cos(w*k*t) + B(:,k)*sin(w*k*t); + q2d = q2d - A(:,k)*w*k.*sin(w*k*t) + B(:,k)*w*k*cos(w*k*t); +end diff --git a/trajectory_optmzn/mixed_traj.m b/trajectory_optmzn/mixed_traj.m index 75d6b6b..8817a56 100644 --- a/trajectory_optmzn/mixed_traj.m +++ b/trajectory_optmzn/mixed_traj.m @@ -20,6 +20,6 @@ qpd = C(:,2) + 2*C(:,3).*t + 3*C(:,4).*t.^2 + ... 4*C(:,5).*t.^3 + 5*C(:,6).*t.^4; qp2d = 2*C(:,3) + 6*C(:,4).*t + 12*C(:,5).*t.^2 + 20*C(:,6).*t.^3; -q = qh+qp; -qd = qhd+qpd; -q2d = qh2d+qp2d; +q = qh + qp; +qd = qhd + qpd; +q2d = qh2d + qp2d; diff --git a/ur_base_params_QRlgr.m b/ur_base_params_QRlgr.m index c0865da..517aa9e 100644 --- a/ur_base_params_QRlgr.m +++ b/ur_base_params_QRlgr.m @@ -1,6 +1,19 @@ % ---------------------------------------------------------------------- % In this script QR decomposition is applied to regressor in closed % form obtained from Lagrange formulation of dynamics. +% +% You should already have a function to compute the regressor matrix of the +% robot full_regressor_UR10E.m +% +% In the beginning you need to choose if you want to include motor dynamics +% (reflected inertia of the motor). A rule of thumb is to include it. +% +% After finding base parametrs the test is performed which compares the +% output torques with base and full parameters for randomly generated data. +% +% Finally, a structure is generated and saved with parameters necessary to +% find base parameters from standard ones, and base regressor from a +% strandard one. % ---------------------------------------------------------------------- % Get robot description run('main_ur.m')