├── .gitignore ├── LICENSE ├── README.md ├── apps ├── direct_dynamic_simulator │ ├── DirectCDPRSimulator.m │ ├── GenerateOutputDirectSimulation.m │ ├── IntegrableDirectDynamics.m │ ├── IntegrableDirectDynamicsSimplified.m │ └── JoinTrajectories.m ├── homing │ ├── ExternalHomingScript.m │ ├── GenerateInitialGuess.m │ ├── Homing33.m │ ├── HomingOptimizationFunction.m │ ├── Reparse.m │ ├── SimulateDataAcquisition.m │ ├── SimulateGoToStartProcedureErroneous.m │ ├── SimulateGoToStartProcedureIdeal.m │ ├── data.log │ └── results.json ├── natural_frequencies_simulator │ ├── CalcAmplitudePhase.m │ ├── CalcLinearizedConfiguration.m │ ├── ExtractInfoFromWS.m │ ├── FunDkUnActL.m │ ├── IntegrableDirectDynamics.m │ ├── IntegrableDirectDynamicsNoInput.m │ ├── IntegrableDirectDynamicsSimplified.m │ └── NaturalFrequenciesCDPRSimulator.m ├── trajectory_planning_under_actuated │ ├── ActuatedDofs.m │ ├── CircleEquation.m │ ├── FindOptimalCoefficients.m │ ├── GenerateOutputUnderActuated.m │ ├── GetDestinations.m │ ├── InverseDynamicPlanner.m │ ├── LineFunction.m │ ├── NormalizedPoly7Coefficients.m │ ├── NormalizedTime.m │ ├── RestToRestCoefficients.m │ ├── StandardInverseSimulator.m │ └── TestTransitionPolynomials.m └── workspace_computation │ ├── CalcCablesStaticTensionNoCheck.m │ ├── CalcOrientationWorkSpace.m │ ├── CalcWorkspace.m │ ├── CalcWorkspaceGraphicalLimits.m │ ├── CalcWorkspaceUnder33.m │ ├── CheckPoseInOrientWorkSpace.m │ ├── DisplayAndSaveWorkspace.m │ ├── FunGsNoCheck.m │ ├── SolveUnderActGeoStatWS.m │ ├── UnderactuatedStaticConstraintNoCheck.m │ └── WorkspaceComputation.m ├── config ├── CreateConfigFile_ToBeUpdated.m ├── Grab_prototype_33.json └── Greenline_prototype.json ├── data ├── planning_results │ ├── DirectRTR1.mat │ ├── DirectSTD1.mat │ ├── InverseRTR1.mat │ ├── InverseRTR2.mat │ ├── InverseRTR3.mat │ ├── InverseSTD1.mat │ ├── InverseSTD2.mat │ └── InverseSTD3.mat └── workspace_files │ ├── Grab_prototype_33_WS.mat │ ├── Grab_prototype_33_max_tension.fig │ ├── Grab_prototype_33_min_tension.fig │ ├── Greenline_prototype_WS.mat │ └── Greenline_prototype_manip.fig └── libs ├── cdpr_model ├── CableParameters.m ├── CableVar.m ├── CalcCableAcceleration.m ├── CalcCableLen.m ├── CalcCableSpeed.m ├── CalcCableVectors.m ├── CalcCableVectorsD.m ├── CalcCablesDynamicTension.m ├── CalcCablesDynamicTensionStateSpace.m ├── CalcCablesStaticTension.m ├── CalcCablesStaticTensionStateSpace.m ├── CalcDynamicLoads.m ├── CalcDynamicLoadsStateSpace.m ├── CalcExternalLoads.m ├── CalcExternalLoadsStateSpace.m ├── CalcKinFirstOrdConstr.m ├── CalcKinSecondOrdConstr.m ├── CalcKinZeroOrdConstr.m ├── CalcPlatformJacobianRow.m ├── CalcPlatformJacobianRowDerivatives.m ├── CalcPulleyVersors.m ├── CalcPulleyVersorsD.m ├── CalcSviwelAngle.m ├── CalcSviwelAngleD.m ├── CalcSviwelAngleD2.m ├── CalcTangentAngle.m ├── CalcTangentAngleD.m ├── CalcTangentAngleD2.m ├── CalcTotalLoads.m ├── CalcTotalLoadsStateSpace.m ├── CdprParameter.m ├── CdprVar.m ├── PlatformParameters.m ├── PlatformVar.m ├── UpdateAccA.m ├── UpdateCableFirstOrd.m ├── UpdateCableSecondOrd.m ├── UpdateCableZeroOrd.m ├── UpdateIKFirstOrd.m ├── UpdateIKSecondOrd.m ├── UpdateIKZeroOrd.m ├── UpdatePlatformAcceleration.m ├── UpdatePlatformPose.m ├── UpdatePlatformVelocity.m ├── UpdatePosA.m └── UpdateVelA.m ├── export_utilities ├── DataLoggerStruct.m ├── GenerateCdprOutput.m ├── GenerateRobotPVT33.m ├── JoinTrajectories.m ├── LoadConfigAndInit.m ├── MakeVideo.m ├── RecordType.m └── UtilitiesType.m ├── numeric ├── Anti.m ├── FiniteDifferentiation.m ├── HuenDiscreteSolver.m ├── HuenSolver.m ├── Interpolate.m ├── MyInv.m ├── MyLowPassFilt.m └── RKSolver.m ├── orientation_geometry ├── DHtfQuaternion.m ├── DHtfRPY.m ├── DHtfTaytBryan.m ├── DHtfTiltTorsion.m ├── DHtfZYZ.m ├── HtfQuaternion.m ├── HtfRPY.m ├── HtfTaytBryan.m ├── HtfTiltTorsion.m ├── HtfZYZ.m ├── Quat2Rot.m ├── Rot2Quat.m ├── Rot2RPY.m ├── Rot2Tayt.m ├── RotRPY.m ├── RotTiltTorsion.m ├── RotX.m ├── RotXYZ.m ├── RotY.m ├── RotZ.m ├── RotZYZ.m └── RotationParametrizations.m ├── prototype_log_parser ├── msgs │ ├── ActuatorStatus.m │ ├── CableRobotMsgs.m │ ├── MotorStatus.m │ └── WinchStatus.m ├── parseCableRobotLogFile.m ├── plot_motor_status.m ├── plot_torques.m └── plot_winch_status.m └── under_actuated ├── AssertStabilityUnderActuated.m ├── CalcJacobianGs.m ├── CalcJacobianL.m ├── CalcJacobianStatic.m ├── CalcJacobianSw.m ├── CalcJacobianTan.m ├── CalcMassMatUnder.m ├── CalcMatrixT.m ├── CalcNaturalFrequencies.m ├── CalcStiffnessMatUnder.m ├── CalcUnactuatedAcceleration.m ├── FunDkGsL.m ├── FunDkGsSwL.m ├── FunDkStat.m ├── FunDkStatL.m ├── FunDkSwL.m ├── FunGs.m ├── GetInfoCdpr.m ├── IntegrableInverseDynamics.m ├── LookForCloseSolution.m ├── MassMatrixNormalization.m ├── UnderActuatedPar.m ├── UnderActuatedVar.m ├── UnderactuatedDynamicsResolution.m └── UnderactuatedStaticConstraint.m /.gitignore: -------------------------------------------------------------------------------- 1 | apps/calibration_identification 2 | data/videos 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # CDPR_MATLAB 2 | 3 | ## Overview 4 | 5 | This repository contains utilities for the simulation of the CDPR prototypes developed at GRAB lab, within the Department of Industrial Engineering of the University of Bologna. 6 | They are organized in folders, divided by their scope: 7 | - _[apps](./apps)_: is a folder which contains the developed applications, organized in subforlders. Every subfolder contains several MATLAB functions, which are only useful for the target application, and MATLAB scripts related to the target application; 8 | - _[config](./config)_: is a folder which contains the config files of the developed GRAB prototypes. The config files are needed in order to run any application in the apps folder; 9 | - _[data](./data)_: is a folder which contains the output of the applications, divided in several subfolders; 10 | - _[libs](./libs)_: is a folder which contains different libraries (cdpr models, export utilities, etc..) which are cross-application and used repository-wide; 11 | 12 | ## Documentation 13 | 14 | Both MATLAB functions and scripts are documented in _[MATLAB documentation style](https://it.mathworks.com/help/matlab/matlab_prog/add-help-for-your-program.html)_. 15 | 16 | Most of the code is already documented, the rest is in development. 17 | 18 | ## Prerequisites 19 | 20 | All the function and scripts have been tested with _[MATLAB2019b](https://mathworks.com/downloads/web_downloads/download_release?release=R2019b)_. 21 | 22 | In order to run the applications, it is necessary to install this _[.json parser](https://github.com/kyamagu/matlab-json)_ 23 | -------------------------------------------------------------------------------- /apps/direct_dynamic_simulator/DirectCDPRSimulator.m: -------------------------------------------------------------------------------- 1 | clear all 2 | close all 3 | clc 4 | addpath('../../config') 5 | addpath('../../data/workspace_files') 6 | addpath('../../libs/cdpr_model') 7 | addpath('../../libs/export_utilities') 8 | addpath('../../libs/numeric') 9 | addpath('../../libs/orientation_geometry') 10 | addpath('../../libs/under_actuated') 11 | folder = '../../data'; 12 | 13 | [cdpr_parameters, cdpr_variables,ws_data, cdpr_outputs,record,utilities] = ... 14 | LoadConfigAndInit("Grab_prototype_33","Direct Simulation"); 15 | 16 | traj_name = '..\..\data\planning_results\InverseSTD'; 17 | traj(1) = load(strcat(traj_name,'1.mat')); 18 | traj(2) = load(strcat(traj_name,'2.mat')); 19 | traj(3) = load(strcat(traj_name,'3.mat')); 20 | 21 | [t,cables,p0] = JoinTrajectories(traj,cdpr_parameters.n_cables,utilities.t_interval); 22 | 23 | % i=0; 24 | % for tt = t 25 | % i=i+1; 26 | % for j=1:cdpr_parameters.n_cables 27 | % l(j,i) = cables(j).length(i)+(2.*rand(1,1)-1).*0.0002; 28 | % if i==1 29 | % dl(j,i) = FiniteDifferentiation(l(j,i),l(j,i),0,t(i),utilities); 30 | % ddl(j,i) = FiniteDifferentiation(dl(j,i),dl(j,i),0,t(i),utilities); 31 | % else 32 | % dl(j,i) = FiniteDifferentiation(l(j,i),l(j,i-1),dl(j,i-1),t(i),utilities); 33 | % ddl(j,i) = FiniteDifferentiation(dl(j,i),dl(j,i-1),ddl(j,i-1),t(i),utilities); 34 | % end 35 | % end 36 | % end 37 | 38 | for j=1:cdpr_parameters.n_cables 39 | spline_id.l(j) = spline(t,cables(j).complete_length); 40 | spline_id.l_d(j) = spline(t,cables(j).complete_speed); 41 | spline_id.l_d2(j) = spline(t,cables(j).complete_acceleration); 42 | 43 | end 44 | 45 | simulation_output = HuenDiscreteSolver(@(time,state) IntegrableDirectDynamics(cdpr_parameters,... 46 | cdpr_variables,utilities,spline_id,time,state),... 47 | 0:utilities.t_interval:t(end),p0); 48 | cdpr_outputs = GenerateOutputDirectSimulation(cdpr_parameters,cdpr_variables,simulation_output,cdpr_outputs); 49 | DataLoggerStruct(cdpr_outputs,folder,'DirectSTD',true,cdpr_parameters,cdpr_variables,record,utilities); 50 | 51 | -------------------------------------------------------------------------------- /apps/direct_dynamic_simulator/GenerateOutputDirectSimulation.m: -------------------------------------------------------------------------------- 1 | function out = GenerateOutputDirectSimulation(cdpr_p,cdpr_v,dir_out,out) 2 | 3 | for i=1:length(dir_out.t) 4 | 5 | cdpr_v = UpdateIKZeroOrd(dir_out.y(1:3,i),dir_out.y(4:cdpr_p.pose_dim,i),cdpr_p,cdpr_v); 6 | cdpr_v = UpdateIKFirstOrd(dir_out.y(cdpr_p.pose_dim+1:cdpr_p.pose_dim+3,i),dir_out.y(cdpr_p.pose_dim+4:end,i),cdpr_p,cdpr_v); 7 | cdpr_v = UpdateIKSecondOrd(dir_out.f(1:3,i),dir_out.f(4:cdpr_p.pose_dim,i),cdpr_p,cdpr_v); 8 | cdpr_v.platform = cdpr_v.platform.UpdateMassMatrixStateSpace(cdpr_p); 9 | cdpr_v = CalcTotalLoadsStateSpace(cdpr_v,cdpr_p); 10 | cdpr_v = CalcCablesDynamicTension(cdpr_v); 11 | out = GetInfoCdpr(cdpr_v,dir_out.t(i),i,out); 12 | 13 | end 14 | 15 | end -------------------------------------------------------------------------------- /apps/direct_dynamic_simulator/IntegrableDirectDynamics.m: -------------------------------------------------------------------------------- 1 | function vect = IntegrableDirectDynamics(cdpr_p,cdpr_v,ut,... 2 | spline_s,time,state) 3 | 4 | for i=1:cdpr_p.n_cables 5 | l(i,1) = ppval(spline_s.l(i),time); 6 | l_d(i,1) = ppval(spline_s.l_d(i),time); 7 | l_d2(i,1) = ppval(spline_s.l_d2(i),time); 8 | end 9 | [cdpr_v,K0] = CalcKinZeroOrdConstr(state(1:3),state(4:6),l,cdpr_p,cdpr_v); 10 | [cdpr_v,K1] = CalcKinFirstOrdConstr(state(7:9),state(10:12),l_d,cdpr_p,cdpr_v); 11 | cdpr_v.platform = cdpr_v.platform.UpdateMassMatrixStateSpace(cdpr_p); 12 | cdpr_v = CalcTotalLoadsStateSpace(cdpr_v,cdpr_p); 13 | 14 | Mat = [cdpr_v.platform.mass_matrix_global_ss cdpr_v.analitic_jacobian'; 15 | cdpr_v.analitic_jacobian zeros(cdpr_p.n_cables)]; 16 | f = [cdpr_v.platform.total_load_ss; 17 | l_d2-cdpr_v.analitic_jacobian_d*state(7:12)-2*ut.baum_zita.*ut.baum_omega.*K1-(ut.baum_omega.^2).*K0]; 18 | v = linsolve(Mat,f); 19 | 20 | vect(1:6,1) = state(7:12); 21 | vect(7:12,1) = v(1:6); 22 | 23 | end -------------------------------------------------------------------------------- /apps/direct_dynamic_simulator/IntegrableDirectDynamicsSimplified.m: -------------------------------------------------------------------------------- 1 | function vect = IntegrableDirectDynamicsSimplified(cdpr_p,cdpr_v,... 2 | spline_s,time,state) 3 | 4 | for i=1:cdpr_p.n_cables 5 | l_d2(i,1) = ppval(spline_s.l_d2(i),time); 6 | end 7 | cdpr_v = UpdateIKZeroOrd(state(1:3),state(4:cdpr_p.pose_dim),cdpr_p,cdpr_v); 8 | cdpr_v = UpdateIKFirstOrd(state(cdpr_p.pose_dim+1:cdpr_p.pose_dim+3),state(cdpr_p.pose_dim+4:end),cdpr_p,cdpr_v); 9 | cdpr_v.platform = cdpr_v.platform.UpdateMassMatrixStateSpace(cdpr_p); 10 | cdpr_v = CalcTotalLoadsStateSpace(cdpr_v,cdpr_p); 11 | 12 | Mat = [cdpr_v.platform.mass_matrix_global_ss cdpr_v.analitic_jacobian'; 13 | cdpr_v.analitic_jacobian zeros(cdpr_p.n_cables)]; 14 | f = [cdpr_v.platform.total_load_ss; 15 | l_d2-cdpr_v.analitic_jacobian_d*state(7:12)]; 16 | v = linsolve(Mat,f); 17 | 18 | vect(1:cdpr_p.pose_dim,1) = state(cdpr_p.pose_dim+1:2*cdpr_p.pose_dim); 19 | vect(cdpr_p.pose_dim+1:2*cdpr_p.pose_dim,1) = v(1:cdpr_p.pose_dim); 20 | 21 | end -------------------------------------------------------------------------------- /apps/direct_dynamic_simulator/JoinTrajectories.m: -------------------------------------------------------------------------------- 1 | function [t,c,p0] = JoinTrajectories(tr,n,dt) 2 | 3 | for i = 1:length(tr) 4 | if (i == 1) 5 | t = tr(i).t; 6 | for j=1:n 7 | c(j).complete_length = tr(i).cables(j).complete_length; 8 | c(j).complete_speed = tr(i).cables(j).complete_speed; 9 | c(j).complete_acceleration = tr(i).cables(j).complete_acceleration; 10 | end 11 | else 12 | t = [t (tr(i).t+t(end))+dt]; 13 | for j=1:n 14 | c(j).complete_length = [c(j).complete_length tr(i).cables(j).complete_length]; 15 | c(j).complete_speed = [c(j).complete_speed tr(i).cables(j).complete_speed]; 16 | c(j).complete_acceleration = [c(j).complete_acceleration tr(i).cables(j).complete_acceleration]; 17 | end 18 | end 19 | t = [t t(end)+dt:dt:t(end)+tr(i).t(end)-dt]; 20 | d1 = size(t); 21 | d2 = size(c(1).complete_length); 22 | diff = d1(2)-d2(2); 23 | if (i == length(tr)) 24 | for j=1:n 25 | c(j).complete_length = [c(j).complete_length c(j).complete_length(end).*ones(1,diff)]; 26 | c(j).complete_speed = [c(j).complete_speed zeros(1,diff)]; 27 | c(j).complete_acceleration = [c(j).complete_acceleration zeros(1,diff)]; 28 | end 29 | else 30 | for j=1:n 31 | c(j).complete_length = [c(j).complete_length c(j).complete_length(end):(tr(i+1).cables(j).complete_length-c(j).complete_length(end))/(diff-1):tr(i+1).cables(j).complete_length]; 32 | c(j).complete_speed = [c(j).complete_speed zeros(1,diff)]; 33 | c(j).complete_acceleration = [c(j).complete_acceleration zeros(1,diff)]; 34 | end 35 | end 36 | end 37 | 38 | p0 = [tr(1).platform.pose(:,1); tr(1).platform.pose_d(:,1)]; 39 | 40 | 41 | end -------------------------------------------------------------------------------- /apps/homing/ExternalHomingScript.m: -------------------------------------------------------------------------------- 1 | clear all 2 | close all 3 | clc 4 | addpath('../../config') 5 | addpath('../../data/workspace_files') 6 | addpath('../../libs/cdpr_model') 7 | addpath('../../libs/export_utilities') 8 | addpath('../../libs/numeric') 9 | addpath('../../libs/orientation_geometry') 10 | addpath('../../libs/under_actuated') 11 | addpath('../../libs/prototype_log_parser') 12 | addpath('../../libs/prototype_log_parser/msgs') 13 | folder = '../../data'; 14 | 15 | [cdpr_parameters, cdpr_variables, ws_parameters, cdpr_outputs,record,utilities] = ... 16 | LoadConfigAndInit("config_calib","Homing"); 17 | 18 | tension_limits = [40 100]; 19 | start = SimulateGoToStartProcedureErroneous... 20 | (cdpr_parameters, cdpr_variables,ws_parameters,record,utilities,tension_limits); 21 | home = SimulateGoToStartProcedureIdeal... 22 | (cdpr_parameters, cdpr_variables,start.pose,record,utilities,tension_limits); 23 | 24 | % Here automatic initial guess for the homing algorithm is generated, 25 | % making use of banal extimation of the workspace center (geometrical 26 | % property of the robot) and acquired data. No need for user interaction. 27 | 28 | [imported_data_coarse, ~] = parseCableRobotLogFile('data.log'); 29 | %[imported_data_coarse, ~] = parseCableRobotLogFile('/tmp/cable-robot-logs/data.log'); 30 | [delta_l,delta_sw] = Reparse(imported_data_coarse.actuator_status.values,... 31 | cdpr_parameters); 32 | init_guess = GenerateInitialGuess(cdpr_parameters,cdpr_variables,delta_l,home,record,utilities); 33 | 34 | init_guess = [start.l;start.sw;init_guess]; 35 | [sol,resnorm,residual,exitflag,output] = lsqnonlin(@(v)HomingOptimizationFunction... 36 | (cdpr_parameters,record,delta_l,delta_sw,v),init_guess ,[],[],utilities.lsqnonlin_options_grad); 37 | l0 = sol(1:cdpr_parameters.n_cables,1); 38 | s0 = sol(cdpr_parameters.n_cables+1:2*cdpr_parameters.n_cables,1); 39 | [pose0,resnormp,residualp,exitflagp,outputp] = lsqnonlin(@(v)FunDkGsSwL... 40 | (cdpr_parameters,l0,s0,v,record),start.pose ,[],[],utilities.lsqnonlin_options_grad); 41 | 42 | j_struct.init_pose = pose0; 43 | json.startup 44 | json.write(j_struct, 'results.json') 45 | fprintf('Results dumped in %s\n', strcat(pwd, '/results.json')) -------------------------------------------------------------------------------- /apps/homing/GenerateInitialGuess.m: -------------------------------------------------------------------------------- 1 | function init = GenerateInitialGuess(cdpr_p,cdpr_v,delta_l,home,rec,ut) 2 | 3 | n = length(delta_l); 4 | init = zeros(cdpr_p.pose_dim*n,1); 5 | for i = 1:n 6 | l = home.l+delta_l(i,:)'; 7 | if (i==1) 8 | in_guess = home.pose; 9 | else 10 | in_guess = solution_calc; 11 | end 12 | solution_calc = fsolve(@(v)FunDkGsL(cdpr_p,l,v,rec),... 13 | in_guess,ut.fsolve_options_grad); 14 | cdpr_v = UpdateIKZeroOrd(solution_calc(1:3),... 15 | solution_calc(4:end),cdpr_p,cdpr_v); 16 | init(cdpr_p.pose_dim*(i-1)+1:i*cdpr_p.pose_dim,1) = cdpr_v.platform.pose; 17 | end 18 | 19 | end -------------------------------------------------------------------------------- /apps/homing/Homing33.m: -------------------------------------------------------------------------------- 1 | clear all 2 | close all 3 | clc 4 | addpath('../../config') 5 | addpath('../../data/workspace_files') 6 | addpath('../../libs/cdpr_model') 7 | addpath('../../libs/export_utilities') 8 | addpath('../../libs/numeric') 9 | addpath('../../libs/orientation_geometry') 10 | addpath('../../libs/under_actuated') 11 | folder = '../../data'; 12 | 13 | [cdpr_parameters, cdpr_variables, ws_parameters, cdpr_outputs,record,utilities] = ... 14 | LoadConfigAndInit("Grab_prototype_33","HomingTest33"); 15 | 16 | % % Generation of the "roughly" estimated home pose 17 | tension_limits = [50 100]; 18 | steps4cable = 5; 19 | start = SimulateGoToStartProcedureErroneous... 20 | (cdpr_parameters, cdpr_variables,ws_parameters,record,utilities,tension_limits); 21 | home = SimulateGoToStartProcedureIdeal... 22 | (cdpr_parameters, cdpr_variables,start.pose,record,utilities,tension_limits); 23 | [delta_l,delta_sw] = SimulateDataAcquisition(cdpr_parameters, cdpr_variables, steps4cable,home,record,utilities,tension_limits); 24 | init_guess = GenerateInitialGuess(cdpr_parameters,cdpr_variables,delta_l,start,record,utilities); 25 | 26 | % Contruct external patameter and initial guess 27 | init_guess = [start.l;start.sw;init_guess]; 28 | [sol,resnorm,residual,exitflag,output] = lsqnonlin(@(v)HomingOptimizationFunction... 29 | (cdpr_parameters,record,delta_l,delta_sw,v),init_guess ,[],[],utilities.lsqnonlin_options_grad); 30 | l0 = sol(1:cdpr_parameters.n_cables,1); 31 | s0 = sol(cdpr_parameters.n_cables+1:2*cdpr_parameters.n_cables,1); 32 | [pose0,resnormp,residualp,exitflagp,outputp] = lsqnonlin(@(v)FunDkGsSwL... 33 | (cdpr_parameters,l0,s0,v,record),start.pose ,[],[],utilities.lsqnonlin_options_grad); 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /apps/homing/HomingOptimizationFunction.m: -------------------------------------------------------------------------------- 1 | function [vector,matrix] = HomingOptimizationFunction(cdpr_p,record,dl,ds,v) 2 | 3 | dim = length(dl); 4 | cdpr_v = CdprVar(cdpr_p.n_cables); 5 | 6 | equations4stage = cdpr_p.n_cables+6; 7 | vector = zeros(dim*equations4stage,1); 8 | matrix = zeros(dim*equations4stage,length(v)); 9 | l_constraint = zeros(cdpr_p.n_cables,1); 10 | sw_constraint = l_constraint; 11 | for j = 1:dim 12 | 13 | pose = v(2*cdpr_p.n_cables+cdpr_p.pose_dim*(j-1)+1:2*cdpr_p.n_cables+j*cdpr_p.pose_dim,1); 14 | cdpr_v = UpdateIKZeroOrd(pose(1:3,1),pose(4:end,1),cdpr_p,cdpr_v); 15 | cdpr_v = CalcExternalLoads(cdpr_v,cdpr_p); 16 | cdpr_v = CalcCablesStaticTension(cdpr_v); 17 | 18 | cdpr_v.underactuated_platform = cdpr_v.underactuated_platform.UpdateGeometricJacobians(cdpr_p.underactuated_platform,cdpr_v.geometric_jacobian); 19 | 20 | for i=1:cdpr_p.n_cables 21 | l_constraint(i) = cdpr_v.cable(i).complete_length-(v(i)+dl(j,i)); 22 | sw_constraint(i) = cdpr_v.cable(i).swivel_ang-(v(i+cdpr_p.n_cables)+ds(j,i)); 23 | end 24 | gs_constraint = cdpr_v.underactuated_platform.geometric_orthogonal'*cdpr_v.platform.ext_load; 25 | 26 | l_jacobian = CalcJacobianL(cdpr_v); 27 | sw_jacobian = CalcJacobianSw(cdpr_v); 28 | gs_jacobian = CalcJacobianGs(cdpr_v); 29 | constr_vect = [l_constraint;sw_constraint;gs_constraint]; 30 | constr_jac = [l_jacobian;sw_jacobian;gs_jacobian]; 31 | 32 | vector((j-1)*equations4stage+1:j*equations4stage,1) = constr_vect; 33 | matrix((j-1)*equations4stage+1:(j-1)*equations4stage+2*cdpr_p.n_cables,1:2*cdpr_p.n_cables) = -eye(2*cdpr_p.n_cables); 34 | matrix((j-1)*equations4stage+1:j*equations4stage,2*cdpr_p.n_cables+cdpr_p.pose_dim*(j-1)+1:2*cdpr_p.n_cables+j*cdpr_p.pose_dim) = constr_jac; 35 | 36 | if (j==1) 37 | record.SetFrame(cdpr_v,cdpr_p); 38 | end 39 | end 40 | 41 | end -------------------------------------------------------------------------------- /apps/homing/Reparse.m: -------------------------------------------------------------------------------- 1 | function [delta_l,delta_sw] = Reparse(s,par) 2 | 3 | l = zeros(length(s.id)/par.n_cables,1); 4 | an = l; 5 | motor_indices = unique(s.id); 6 | for i=1:par.n_cables 7 | id = find(s.id == motor_indices(i)); 8 | for j = 1:length(id) 9 | l(j,i) = s.cable_length(id(j)); 10 | an(j,i) = s.pulley_angle(id(j)); 11 | end 12 | end 13 | delta_l = l; 14 | delta_sw = an; 15 | 16 | end -------------------------------------------------------------------------------- /apps/homing/SimulateDataAcquisition.m: -------------------------------------------------------------------------------- 1 | function [delta_l,delta_sw] = SimulateDataAcquisition(cdpr_p, cdpr_v, steps,home,rec,ut,T) 2 | 3 | deltaT = (T(2)-T(1))/steps; 4 | index = 0; 5 | delta_l = zeros(cdpr_p.n_cables*2*steps,cdpr_p.n_cables); 6 | delta_sw = delta_l; 7 | for i = 1:cdpr_p.n_cables 8 | for procedure_phase = 1:2 9 | for j=1:steps 10 | index = index+1; 11 | tension_vector = T(1).*ones(cdpr_p.n_cables,1); 12 | if procedure_phase==1 13 | tension_vector(i) = T(1)+deltaT.*j; 14 | else 15 | tension_vector(i) = T(2)-deltaT.*j; 16 | end 17 | if (index==1) 18 | in_guess = home.pose; 19 | else 20 | in_guess = solution_calc; 21 | end 22 | solution_calc = fsolve(@(v)FunDkStat(cdpr_p,tension_vector,v,rec),... 23 | in_guess,ut.fsolve_options_grad); 24 | cdpr_v = UpdateIKZeroOrd(solution_calc(1:3),... 25 | solution_calc(4:end),cdpr_p,cdpr_v); 26 | for k=1:cdpr_p.n_cables 27 | delta_l(index,k) = cdpr_v.cable(k).complete_length-home.l(k); 28 | delta_sw(index,k) = cdpr_v.cable(k).swivel_ang-home.sw(k); 29 | end 30 | end 31 | end 32 | end 33 | 34 | end -------------------------------------------------------------------------------- /apps/homing/SimulateGoToStartProcedureErroneous.m: -------------------------------------------------------------------------------- 1 | function start = SimulateGoToStartProcedureErroneous(cdpr_p, cdpr_v, ws_p,rec,ut,T) 2 | 3 | reference_position = ws_p.workspace_center; 4 | initial_guess = LookForCloseSolution(cdpr_p,ws_p,reference_position); 5 | reference_angle = fsolve(@(v) FunGs(cdpr_p,reference_position,v,rec),... 6 | initial_guess,ut.fsolve_options_grad); 7 | 8 | initial_guess = [reference_position;reference_angle]; 9 | 10 | start.pose = fsolve(@(v) FunDkStat(cdpr_p,T(1).*0.8.*ones(cdpr_p.n_cables,1),v,rec),... 11 | initial_guess,ut.fsolve_options_grad); 12 | cdpr_v = UpdateIKZeroOrd(start.pose(1:3),... 13 | start.pose(4:end),cdpr_p,cdpr_v); 14 | 15 | start.l = zeros(cdpr_p.n_cables,1); 16 | start.sw = zeros(cdpr_p.n_cables,1); 17 | for i=1:cdpr_p.n_cables 18 | start.l(i,1) = cdpr_v.cable(i).complete_length; 19 | start.sw(i,1) = cdpr_v.cable(i).swivel_ang; 20 | end 21 | 22 | 23 | end -------------------------------------------------------------------------------- /apps/homing/SimulateGoToStartProcedureIdeal.m: -------------------------------------------------------------------------------- 1 | function home = SimulateGoToStartProcedureIdeal(cdpr_p, cdpr_v, start_pose,rec,ut,T) 2 | 3 | home.pose = fsolve(@(v) FunDkStat(cdpr_p,T(1).*ones(cdpr_p.n_cables,1),v,rec),... 4 | start_pose,ut.fsolve_options_grad); 5 | cdpr_v = UpdateIKZeroOrd(home.pose(1:3),... 6 | home.pose(4:end),cdpr_p,cdpr_v); 7 | 8 | home.l = zeros(cdpr_p.n_cables,1); 9 | home.sw = zeros(cdpr_p.n_cables,1); 10 | for i=1:cdpr_p.n_cables 11 | home.l(i,1) = cdpr_v.cable(i).complete_length; 12 | home.sw(i,1) = cdpr_v.cable(i).swivel_ang; 13 | end 14 | 15 | 16 | end -------------------------------------------------------------------------------- /apps/homing/data.log: -------------------------------------------------------------------------------- 1 | 3,87.536,0,10,5925271,346,-174,-5.9936e-08,11754,1,0 2 | 3,87.536,4,10,5132937,253,-167,-2.9968e-07,11118,1,0 3 | 3,87.536,6,10,5243945,209,-76,5.9936e-08,-11982,1,0 4 | 3,90.6806,0,10,5924247,1143,-240,-6.14344e-05,11754,1,0 5 | 3,90.6806,4,10,5132935,-515,-168,-4.19552e-07,11118,1,0 6 | 3,90.6806,6,10,5243946,-1060,-75,1.19872e-07,-11982,1,0 7 | 3,94.5949,0,10,5880571,793,-308,-0.0026792,11727,1,-0.000647148 8 | 3,94.5949,4,10,5132933,-330,-168,-5.39424e-07,11116,1,-4.79369e-05 9 | 3,94.5949,6,10,5243948,-650,-77,2.39744e-07,-11979,1,7.19053e-05 10 | 3,109.149,0,10,5336003,380,-373,-0.0353184,11719,1,-0.000838896 11 | 3,109.149,4,10,5132902,1082,-166,-2.39744e-06,10720,1,-0.00953944 12 | 3,109.149,6,10,5243952,5,-77,4.79488e-07,-11501,1,0.0115288 13 | 3,120.581,0,10,1561942,790,-442,-0.261521,11814,1,0.00143811 14 | 3,120.581,4,10,5132826,256,-167,-6.95258e-06,7418,1,-0.0886833 15 | 3,120.581,6,10,5243953,93,-75,5.39424e-07,-7019,1,0.118955 16 | 3,123.79,0,10,1561994,-71,-372,-0.261517,11814,1,0.00143811 17 | 3,123.79,4,10,5132824,127,-169,-7.07245e-06,7418,1,-0.0886833 18 | 3,123.79,6,10,5243953,671,-75,5.39424e-07,-7018,1,0.118979 19 | 3,126.974,0,10,1562223,678,-308,-0.261504,11814,1,0.00143811 20 | 3,126.974,4,10,5132823,745,-168,-7.13238e-06,7418,1,-0.0886833 21 | 3,126.974,6,10,5243953,56,-76,5.39424e-07,-7017,1,0.119003 22 | 3,130.168,0,10,1563533,681,-240,-0.261425,11814,1,0.00143811 23 | 3,130.168,4,10,5132821,185,-168,-7.25226e-06,7418,1,-0.0886833 24 | 3,130.168,6,10,5243954,470,-75,5.9936e-07,-7016,1,0.119027 25 | 3,134.022,0,10,1570539,-452,-174,-0.261005,11830,1,0.0018216 26 | 3,134.022,4,10,5132819,-55,-170,-7.37213e-06,7418,1,-0.0886833 27 | 3,134.022,6,10,5243954,-6,-75,5.9936e-07,-7016,1,0.119027 28 | 3,137.051,0,10,1570684,48,-176,-0.260997,11831,1,0.00184557 29 | 3,137.051,4,10,5132818,123,-167,-7.43206e-06,7418,1,-0.0886833 30 | 3,137.051,6,10,5243954,10,-77,5.9936e-07,-7016,1,0.119027 31 | 3,140.244,0,10,1570764,606,-175,-0.260992,11831,1,0.00184557 32 | 3,140.244,4,10,5131899,-5,-237,-6.25132e-05,7418,1,-0.0886833 33 | 3,140.244,6,10,5243955,999,-75,6.59296e-07,-7016,1,0.119027 34 | 3,144.887,0,10,1570851,228,-176,-0.260987,11835,1,0.00194144 35 | 3,144.887,4,10,5092015,384,-305,-0.002453,7418,1,-0.0886833 36 | 3,144.887,6,10,5243955,9,-76,6.59296e-07,-6993,1,0.119579 37 | 3,161.302,0,10,1571223,5,-175,-0.260964,11677,1,-0.00184557 38 | 3,161.302,4,10,4561368,931,-372,-0.0342579,8051,1,-0.0735112 39 | 3,161.302,6,10,5243957,6,-76,7.79168e-07,-6755,1,0.125283 40 | 3,176.427,0,10,1571918,-47,-175,-0.260923,8860,1,-0.0693647 41 | 3,176.427,4,10,533693,-643,-441,-0.275661,6517,1,-0.110279 42 | 3,176.427,6,10,5243957,-196,-77,7.79168e-07,-4509,1,0.179116 43 | 3,179.632,0,10,1571924,-1097,-174,-0.260922,8860,1,-0.0693647 44 | 3,179.632,4,10,533762,-6,-372,-0.275656,6517,1,-0.110279 45 | 3,179.632,6,10,5243958,-689,-77,8.39104e-07,-4509,1,0.179116 46 | 3,182.845,0,10,1571935,-320,-176,-0.260922,8860,1,-0.0693647 47 | 3,182.845,4,10,534067,108,-304,-0.275638,6517,1,-0.110279 48 | 3,182.845,6,10,5243958,-1069,-76,8.39104e-07,-4509,1,0.179116 49 | 3,186.035,0,10,1571943,-49,-175,-0.260921,8860,1,-0.0693647 50 | 3,186.035,4,10,536881,-323,-236,-0.275469,6517,1,-0.110279 51 | 3,186.035,6,10,5243959,-259,-76,8.9904e-07,-4509,1,0.179116 52 | 3,189.258,0,10,1571944,-253,-174,-0.260921,8861,1,-0.0693407 53 | 3,189.258,4,10,558931,-239,-170,-0.274148,6518,1,-0.110255 54 | 3,189.258,6,10,5243960,669,-76,9.58976e-07,-4510,1,0.179092 55 | 3,192.293,0,10,1571944,-297,-173,-0.260921,8861,1,-0.0693407 56 | 3,192.293,4,10,559200,795,-168,-0.274132,6518,1,-0.110255 57 | 3,192.293,6,10,5243959,-8,-76,8.9904e-07,-4510,1,0.179092 58 | 3,195.486,0,10,1571948,793,-176,-0.260921,8861,1,-0.0693407 59 | 3,195.486,4,10,559372,215,-168,-0.274121,6518,1,-0.110255 60 | 3,195.486,6,10,5243849,139,-169,-5.69392e-06,-4510,1,0.179092 61 | 3,198.709,0,10,1571948,-156,-176,-0.260921,8861,1,-0.0693407 62 | 3,198.709,4,10,559492,-497,-169,-0.274114,6517,1,-0.110279 63 | 3,198.709,6,10,5242642,-896,-258,-7.80367e-05,-4510,1,0.179092 64 | 3,212.734,0,10,1571959,-126,-176,-0.26092,8644,1,-0.0745419 65 | 3,212.734,4,10,559900,941,-167,-0.27409,6390,1,-0.113323 66 | 3,212.734,6,10,5111148,-932,-350,-0.00795926,-4406,1,0.181585 67 | 3,226.733,0,10,1571966,-186,-175,-0.26092,1436,1,-0.247306 68 | 3,226.733,4,10,562692,463,-169,-0.273922,1646,1,-0.227029 69 | 3,226.733,6,10,702491,-3,-439,-0.272197,-2526,1,0.226646 70 | 3,229.958,0,10,1571969,-210,-176,-0.26092,1436,1,-0.247306 71 | 3,229.958,4,10,562812,-471,-168,-0.273915,1646,1,-0.227029 72 | 3,229.958,6,10,702572,644,-348,-0.272192,-2526,1,0.226646 73 | 3,233.158,0,10,1571967,351,-174,-0.26092,1436,1,-0.247306 74 | 3,233.158,4,10,562908,-540,-167,-0.27391,1646,1,-0.227029 75 | 3,233.158,6,10,703167,0,-259,-0.272156,-2526,1,0.226646 76 | 3,236.383,0,10,1571967,-136,-174,-0.26092,1436,1,-0.247306 77 | 3,236.383,4,10,562984,546,-167,-0.273905,1646,1,-0.227029 78 | 3,236.383,6,10,712554,-633,-168,-0.271593,-2533,1,0.226478 79 | 3,241.703,0,10,1571969,537,-174,-0.26092,1664,1,-0.241842 80 | 3,241.703,4,10,563057,-340,-168,-0.273901,1798,1,-0.223386 81 | 3,241.703,6,10,893562,526,-75,-0.260744,-2552,1,0.226022 82 | -------------------------------------------------------------------------------- /apps/homing/results.json: -------------------------------------------------------------------------------- 1 | {"init_pose":[[-0.3099441445892487],[1.1703092452174442],[-0.6069822466813284],[0.0020002314284270745],[0.03629136761548845],[-0.0018345737090816267]]} -------------------------------------------------------------------------------- /apps/natural_frequencies_simulator/CalcAmplitudePhase.m: -------------------------------------------------------------------------------- 1 | function data = CalcAmplitudePhase(data) 2 | 3 | sol_pos = linsolve(data.normal_modes,data.start_conf_un_act-data.ang_par); 4 | sol_vel = linsolve(data.normal_modes.*(2.*pi.*data.nat_freq'),data.start_conf_un_act_der); 5 | for i=1:length(sol_pos) 6 | data.phase(i,1) = atan2(sol_vel(i),sol_pos(i)); 7 | data.amplitude(i,1) = (sol_vel(i)+sol_pos(i))./(cos(data.phase(i,1))+sin(data.phase(i,1))); 8 | end 9 | 10 | end -------------------------------------------------------------------------------- /apps/natural_frequencies_simulator/CalcLinearizedConfiguration.m: -------------------------------------------------------------------------------- 1 | function [p,dp,ddp] = CalcLinearizedConfiguration(data,t) 2 | 3 | 4 | p_mat = data.normal_modes.*data.amplitude'; 5 | dp_mat = -data.normal_modes.*(2.*pi.*(data.nat_freq.*data.amplitude)'); 6 | ddp_mat = -data.normal_modes.*(4.*(pi^2).*(data.nat_freq.*data.nat_freq.*data.amplitude)'); 7 | 8 | p_v = cos(2.*pi.*data.nat_freq.*t-data.phase); 9 | dp_v = sin(2.*pi.*data.nat_freq.*t-data.phase); 10 | ddp_v = cos(2.*pi.*data.nat_freq.*t-data.phase); 11 | 12 | p = 180/pi.*(p_mat*p_v+data.ang_par); 13 | dp = 180/pi.*dp_mat*dp_v; 14 | ddp = 180/pi.*ddp_mat*ddp_v; 15 | 16 | end -------------------------------------------------------------------------------- /apps/natural_frequencies_simulator/ExtractInfoFromWS.m: -------------------------------------------------------------------------------- 1 | function data = ExtractInfoFromWS(cdpr_v,cdpr_p,ws_data,ut,rec,i,c,dc) 2 | 3 | data.pos = ws_data.position(:,i); 4 | data.rot_mat = ws_data.rot_mat(:,:,i); 5 | data.ang_par = cdpr_p.RotToPar(data.rot_mat); 6 | 7 | cdpr_v = UpdateIKZeroOrd(data.pos,data.ang_par,cdpr_p,cdpr_v); 8 | cdpr_v.underactuated_platform = cdpr_v.underactuated_platform.UpdateAnaliticJacobians... 9 | (cdpr_p.underactuated_platform,cdpr_v.analitic_jacobian); 10 | cdpr_v = cdpr_v.UpdateTransformationMatrix(cdpr_p); 11 | cdpr_v.underactuated_platform = cdpr_v.underactuated_platform.UpdateReducedTransformationMatrix(cdpr_v.D_mat); 12 | 13 | data.nat_freq = ws_data.nat_freq(:,i); 14 | data.normal_modes = linsolve(cdpr_v.underactuated_platform.Gamma_mat,ws_data.normal_modes(:,:,i)); 15 | data.l = cdpr_v.cable_vector; 16 | 17 | sign_p = 2*rand(3,1)-1; 18 | sign_p = sign_p./abs(sign_p); 19 | data.start_conf_un_act = data.ang_par+c.*(pi/180).*sign_p; 20 | start_conf_act = fsolve(@(v) FunDkUnActL(cdpr_p,data.l,data.start_conf_un_act,v,rec),... 21 | data.pos,ut.fsolve_options_grad); 22 | data.p0 = [start_conf_act;data.start_conf_un_act]; 23 | cdpr_v = UpdateIKZeroOrd(start_conf_act,data.start_conf_un_act,cdpr_p,cdpr_v); 24 | cdpr_v.underactuated_platform = cdpr_v.underactuated_platform.UpdateAnaliticJacobians... 25 | (cdpr_p.underactuated_platform,cdpr_v.analitic_jacobian); 26 | 27 | sign_v = rand(3,1); 28 | sign_v = sign_v./abs(sign_v); 29 | data.start_conf_un_act_der = dc.*(pi/180).*sign_v; 30 | data.v0 = cdpr_v.underactuated_platform.analitic_orthogonal*data.start_conf_un_act_der; 31 | 32 | data = CalcAmplitudePhase(data); 33 | 34 | end -------------------------------------------------------------------------------- /apps/natural_frequencies_simulator/FunDkUnActL.m: -------------------------------------------------------------------------------- 1 | function [vector,matrix] = FunDkUnActL(cdpr_p,complete_length,un_act_vars,variables,varargin) 2 | 3 | cdpr_v = CdprVar(cdpr_p.n_cables); 4 | 5 | cdpr_v.underactuated_platform = cdpr_v.underactuated_platform.SetVars(0,variables,... 6 | un_act_vars); 7 | pose = cdpr_v.underactuated_platform.RecomposeVars(0,cdpr_p.underactuated_platform); 8 | 9 | [cdpr_v,constraint_l] = CalcKinZeroOrdConstr(pose(1:3),pose(4:end),complete_length,cdpr_p,cdpr_v); 10 | 11 | l_jacobian = CalcJacobianL(cdpr_v); 12 | vector = constraint_l; 13 | matrix = l_jacobian(:,1:cdpr_p.n_cables); 14 | 15 | if (~isempty(varargin)) 16 | varargin{1}.SetFrame(cdpr_v,cdpr_p); 17 | end 18 | 19 | end -------------------------------------------------------------------------------- /apps/natural_frequencies_simulator/IntegrableDirectDynamics.m: -------------------------------------------------------------------------------- 1 | function vect = IntegrableDirectDynamics(cdpr_p,cdpr_v,ut,... 2 | spline_s,time,state) 3 | 4 | for i=1:cdpr_p.n_cables 5 | l(i,1) = ppval(spline_s.l(i),time); 6 | l_d(i,1) = ppval(spline_s.l_d(i),time); 7 | l_d2(i,1) = ppval(spline_s.l_d2(i),time); 8 | end 9 | [cdpr_v,K0] = CalcKinZeroOrdConstr(state(1:3),state(4:6),l,cdpr_p,cdpr_v); 10 | [cdpr_v,K1] = CalcKinFirstOrdConstr(state(7:9),state(10:12),l_d,cdpr_p,cdpr_v); 11 | cdpr_v.platform = cdpr_v.platform.UpdateMassMatrixStateSpace(cdpr_p); 12 | cdpr_v = CalcTotalLoadsStateSpace(cdpr_v,cdpr_p); 13 | 14 | Mat = [cdpr_v.platform.mass_matrix_global_ss cdpr_v.analitic_jacobian'; 15 | cdpr_v.analitic_jacobian zeros(cdpr_p.n_cables)]; 16 | f = [cdpr_v.platform.total_load_ss; 17 | l_d2-cdpr_v.analitic_jacobian_d*state(7:12)-2*ut.baum_zita.*ut.baum_omega.*K1-(ut.baum_omega.^2).*K0]; 18 | v = linsolve(Mat,f); 19 | 20 | vect(1:6,1) = state(7:12); 21 | vect(7:12,1) = v(1:6); 22 | 23 | end -------------------------------------------------------------------------------- /apps/natural_frequencies_simulator/IntegrableDirectDynamicsNoInput.m: -------------------------------------------------------------------------------- 1 | function vect = IntegrableDirectDynamicsNoInput(cdpr_p,cdpr_v,ut,... 2 | l,time,state) 3 | 4 | [cdpr_v,K0] = CalcKinZeroOrdConstr(state(1:3),state(4:6),l,cdpr_p,cdpr_v); 5 | [cdpr_v,K1] = CalcKinFirstOrdConstr(state(7:9),state(10:12),zeros(cdpr_p.n_cables),cdpr_p,cdpr_v); 6 | cdpr_v.platform = cdpr_v.platform.UpdateMassMatrixStateSpace(cdpr_p); 7 | cdpr_v = CalcTotalLoadsStateSpace(cdpr_v,cdpr_p); 8 | 9 | Mat = [cdpr_v.platform.mass_matrix_global_ss cdpr_v.analitic_jacobian'; 10 | cdpr_v.analitic_jacobian zeros(cdpr_p.n_cables)]; 11 | f = [cdpr_v.platform.total_load_ss; 12 | -cdpr_v.analitic_jacobian_d*state(7:12)-2*ut.baum_zita.*ut.baum_omega.*K1-(ut.baum_omega.^2).*K0]; 13 | v = linsolve(Mat,f); 14 | 15 | vect(1:6,1) = state(7:12); 16 | vect(7:12,1) = v(1:6); 17 | 18 | end -------------------------------------------------------------------------------- /apps/natural_frequencies_simulator/IntegrableDirectDynamicsSimplified.m: -------------------------------------------------------------------------------- 1 | function vect = IntegrableDirectDynamicsSimplified(cdpr_p,cdpr_v,... 2 | spline_s,time,state) 3 | 4 | for i=1:cdpr_p.n_cables 5 | l_d2(i,1) = ppval(spline_s.l_d2(i),time); 6 | end 7 | cdpr_v = UpdateIKZeroOrd(state(1:3),state(4:cdpr_p.pose_dim),cdpr_p,cdpr_v); 8 | cdpr_v = UpdateIKFirstOrd(state(cdpr_p.pose_dim+1:cdpr_p.pose_dim+3),state(cdpr_p.pose_dim+4:end),cdpr_p,cdpr_v); 9 | cdpr_v.platform = cdpr_v.platform.UpdateMassMatrixStateSpace(cdpr_p); 10 | cdpr_v = CalcTotalLoadsStateSpace(cdpr_v,cdpr_p); 11 | 12 | Mat = [cdpr_v.platform.mass_matrix_global_ss cdpr_v.analitic_jacobian'; 13 | cdpr_v.analitic_jacobian zeros(cdpr_p.n_cables)]; 14 | f = [cdpr_v.platform.total_load_ss; 15 | l_d2-cdpr_v.analitic_jacobian_d*state(7:12)]; 16 | v = linsolve(Mat,f); 17 | 18 | vect(1:cdpr_p.pose_dim,1) = state(cdpr_p.pose_dim+1:2*cdpr_p.pose_dim); 19 | vect(cdpr_p.pose_dim+1:2*cdpr_p.pose_dim,1) = v(1:cdpr_p.pose_dim); 20 | 21 | end -------------------------------------------------------------------------------- /apps/natural_frequencies_simulator/NaturalFrequenciesCDPRSimulator.m: -------------------------------------------------------------------------------- 1 | clear all 2 | close all 3 | clc 4 | addpath('../../config') 5 | addpath('../../data/workspace_files') 6 | addpath('../../libs/cdpr_model') 7 | addpath('../../libs/export_utilities') 8 | addpath('../../libs/numeric') 9 | addpath('../../libs/orientation_geometry') 10 | addpath('../../libs/under_actuated') 11 | folder = '../../data'; 12 | 13 | [cdpr_parameters, cdpr_variables,ws_data, cdpr_outputs,record,utilities] = ... 14 | LoadConfigAndInit("Grab_prototype_33","DynamicPlanning"); 15 | 16 | index = randi(ws_data.counter); 17 | scale_fact_pos = 1; %max amplitude degree 18 | scale_fact_vel = 0; %max amplitude degree/s 19 | t_end = 10; %sim time 20 | linearized_data = ExtractInfoFromWS(cdpr_variables,cdpr_parameters,ws_data,... 21 | utilities,record,index,scale_fact_pos,scale_fact_vel); 22 | i = 0; 23 | 24 | for t = 0:utilities.t_interval:t_end 25 | i=i+1; 26 | time(i,1) = t; 27 | [p(:,i),dp(:,i),ddp(:,i)] = CalcLinearizedConfiguration(linearized_data,t); 28 | end 29 | 30 | sol_real_simp = HuenDiscreteSolver(@(time,state) IntegrableDirectDynamicsNoInput(cdpr_parameters,... 31 | cdpr_variables,utilities,linearized_data.l,time,state),... 32 | 0:utilities.t_interval:t_end,[linearized_data.p0;linearized_data.v0]); 33 | real_sol = 180/pi.*sol_real_simp.y(4:6,:); 34 | dreal_sol = 180/pi.*sol_real_simp.y(10:12,:); 35 | ddreal_sol = 180/pi.*sol_real_simp.f(4:6,:); 36 | 37 | figure 38 | subplot(3,1,1) 39 | plot(time,p(1,:),time,real_sol(1,:)) 40 | subplot(3,1,2) 41 | plot(time,p(2,:),time,real_sol(2,:)) 42 | subplot(3,1,3) 43 | plot(time,p(3,:),time,real_sol(3,:)) 44 | 45 | diff_p = (real_sol-p); 46 | diff_dp = (dreal_sol-dp); 47 | diff_ddp = (ddreal_sol-ddp); -------------------------------------------------------------------------------- /apps/trajectory_planning_under_actuated/ActuatedDofs.m: -------------------------------------------------------------------------------- 1 | function dofs = ActuatedDofs(c,tau,geoFun,p0,p1) 2 | 3 | vt = [tau(1)^4 tau(1)^5 tau(1)^6 tau(1)^7]; 4 | dvt = [tau(1)^3 tau(1)^4 tau(1)^5 tau(1)^6]; 5 | ddvt = [tau(1)^2 tau(1)^3 tau(1)^4 tau(1)^5]; 6 | 7 | g(1) = vt*c.c; 8 | g(2) = dvt*c.cDerivative; 9 | g(3) = ddvt*c.cDerivative2; 10 | 11 | geometricDofs = geoFun(g(1),p0,p1,c); 12 | for i=1:length(p0) 13 | dofs(:,i) = [geometricDofs(1,i);... 14 | geometricDofs(2,i)*g(2)*tau(2);... 15 | (geometricDofs(3,i)*g(2)^2+geometricDofs(2,i)*g(3))*tau(2)^2+... 16 | geometricDofs(2,i)*g(2)*tau(3)]; 17 | end 18 | 19 | end -------------------------------------------------------------------------------- /apps/trajectory_planning_under_actuated/CircleEquation.m: -------------------------------------------------------------------------------- 1 | function mat = CircleEquation(c,r,u,thE,t) 2 | 3 | n = Anti(u(:,1))*u(:,2); 4 | n = Anti(n)*u(:,1); 5 | n = n./norm(n); 6 | for i=1:3 7 | mat(:,i) = [c(i)+r*u(i,1)*cos(thE*t)+r*n(i)*sin(thE*t); 8 | -thE*r*u(i,1)*sin(thE*t)+thE*r*n(i)*cos(thE*t); 9 | -thE^2*r*u(i,1)*cos(thE*t)-thE^2*r*n(i)*sin(thE*t)]; 10 | end 11 | 12 | end -------------------------------------------------------------------------------- /apps/trajectory_planning_under_actuated/FindOptimalCoefficients.m: -------------------------------------------------------------------------------- 1 | function vector = FindOptimalCoefficients(k,cdpr_p,cdpr_v,ut,sim_data,i,geom_fun,record) 2 | 3 | sol = ode45(@(time,orientation) IntegrableInverseDynamics(cdpr_p,cdpr_v,... 4 | sim_data,i,time,orientation,sim_data.dt(i),k,geom_fun),[0 sim_data.dt(i)],... 5 | [sim_data.p(4:6,i);0;0;0],ut.ode45_options); 6 | [y yp] = deval(sol,sol.x); 7 | vector = [y(1:cdpr_p.n_cables,end)-sim_data.p(4:6,i+1); 8 | y(cdpr_p.n_cables+1:2*cdpr_p.n_cables,end); 9 | yp(cdpr_p.n_cables+1:2*cdpr_p.n_cables,end)]; 10 | 11 | % sol = RKSolver(@(time,orientation) IntegrableInverseDynamics33(cdpr_p,cdpr_v,... 12 | % sim_data,i,time,orientation,sim_data.dt(i),k,geom_fun),... 13 | % 0:ut.t_interval*10:sim_data.dt(i),[sim_data.p(4:6,i);0;0;0]); 14 | % vector = [sol.y(1:6-cdpr_p.n_cables,end)-sim_data.p(4:6,i+1); 15 | % sol.y(7-cdpr_p.n_cables:2*(6-cdpr_p.n_cables),end); 16 | % sol.f(:,end)]; 17 | 18 | 19 | end -------------------------------------------------------------------------------- /apps/trajectory_planning_under_actuated/GenerateOutputUnderActuated.m: -------------------------------------------------------------------------------- 1 | function out = GenerateOutputUnderActuated(ind,cdpr_p,cdpr_v,sim_data,geom_fun,ut) 2 | 3 | timeSpan = 0:ut.t_interval:sim_data.dt(ind); 4 | start_coordinate = [sim_data.p(4:6,ind);0;0;0]; 5 | sol = RKSolver(@(time,orientation) IntegrableInverseDynamics(cdpr_p,cdpr_v,... 6 | sim_data,ind,time,orientation,sim_data.dt(ind),sim_data.coeff(:,ind),geom_fun),... 7 | timeSpan,start_coordinate); 8 | out = struct(); 9 | for i=1:length(timeSpan) 10 | 11 | pStart = sim_data.p(1:cdpr_p.n_cables,ind); 12 | pEnd = sim_data.p(1:cdpr_p.n_cables,ind+1); 13 | normalizedTime = NormalizedTime(sim_data.coeff(:,ind),timeSpan(i),sim_data.dt(ind)); 14 | dofs = ActuatedDofs(sim_data,normalizedTime,... 15 | geom_fun,pStart,pEnd); 16 | 17 | cdpr_v = UpdateIKZeroOrd(dofs(1,:)',sol.y(1:3,i),cdpr_p,cdpr_v); 18 | cdpr_v = UpdateIKFirstOrd(dofs(2,:)',sol.y(4:6,i),cdpr_p,cdpr_v); 19 | cdpr_v = UpdateIKSecondOrd(dofs(3,:)',sol.f(1:3,i),cdpr_p,cdpr_v); 20 | cdpr_v.platform = cdpr_v.platform.UpdateMassMatrixStateSpace(cdpr_p); 21 | cdpr_v = CalcTotalLoadsStateSpace(cdpr_v,cdpr_p); 22 | cdpr_v = CalcCablesDynamicTension(cdpr_v); 23 | out = GetInfoCdpr(cdpr_v,timeSpan(i),i,out); 24 | 25 | end 26 | 27 | end -------------------------------------------------------------------------------- /apps/trajectory_planning_under_actuated/GetDestinations.m: -------------------------------------------------------------------------------- 1 | function data = GetDestinations(data,ws_par,cdpr_p,record,ut) 2 | 3 | data.pNumber = 4; 4 | 5 | data.p(1:3,1) = ws_par.workspace_center-[0.3;0;0.5]; 6 | initial_guess = LookForCloseSolution(cdpr_p,ws_par,data.p(1:3,1)); 7 | data.p(4:6,1) = fsolve(@(v) FunGs(cdpr_p,data.p(1:3,1),v,record),... 8 | initial_guess,ut.fsolve_options_grad); 9 | 10 | data.dt(1) = 2.5; 11 | 12 | data.p(1:3,2) = ws_par.workspace_center+[0.5;0;-0.5]; 13 | initial_guess = LookForCloseSolution(cdpr_p,ws_par,data.p(1:3,2)); 14 | data.p(4:6,2) = fsolve(@(v) FunGs(cdpr_p,data.p(1:3,2),v,record),... 15 | initial_guess,ut.fsolve_options_grad); 16 | 17 | data.dt(2) = 2; 18 | 19 | data.p(1:3,3) = ws_par.workspace_center+[0.1;0;0.1]; 20 | initial_guess = LookForCloseSolution(cdpr_p,ws_par,data.p(1:3,3)); 21 | data.p(4:6,3) = fsolve(@(v) FunGs(cdpr_p,data.p(1:3,3),v,record),... 22 | initial_guess,ut.fsolve_options_grad); 23 | 24 | data.dt(3) = 2; 25 | 26 | data.p(1:3,4) = data.p(1:3,1); 27 | data.p(4:6,4) = data.p(4:6,1); 28 | 29 | end -------------------------------------------------------------------------------- /apps/trajectory_planning_under_actuated/InverseDynamicPlanner.m: -------------------------------------------------------------------------------- 1 | % InverseDynamicPlanner33 2 | clear all 3 | close all 4 | clc 5 | addpath('../../config') 6 | addpath('../../data/workspace_files') 7 | addpath('../../libs/cdpr_model') 8 | addpath('../../libs/export_utilities') 9 | addpath('../../libs/numeric') 10 | addpath('../../libs/orientation_geometry') 11 | addpath('../../libs/under_actuated') 12 | folder = '../../data'; 13 | 14 | [cdpr_parameters, cdpr_variables, ws_parameters, cdpr_outputs,record,utilities] = ... 15 | LoadConfigAndInit("Grab_prototype_33","DynamicPlanning"); 16 | 17 | % fare la cartella output per la roba enorme con dentro un .keep 18 | 19 | simulationData = struct(); 20 | geometricFunction = @LineFunction; 21 | simulationData = NormalizedPoly7Coefficients(1,simulationData); 22 | simulationData = GetDestinations(simulationData,ws_parameters,cdpr_parameters,record,utilities); 23 | 24 | [outputDataRTR] = RestToRestCoefficients(cdpr_parameters,cdpr_variables,... 25 | simulationData,geometricFunction,utilities,record); 26 | [outputDataSTD] = StandardInverseSimulator(cdpr_parameters,cdpr_variables,... 27 | simulationData,geometricFunction,utilities); 28 | 29 | DataLoggerStruct(outputDataRTR,folder,'InverseRTR',true,cdpr_parameters,cdpr_variables,record,utilities); 30 | DataLoggerStruct(outputDataSTD,folder,'InverseSTD',true,cdpr_parameters,cdpr_variables,record,utilities); 31 | -------------------------------------------------------------------------------- /apps/trajectory_planning_under_actuated/LineFunction.m: -------------------------------------------------------------------------------- 1 | function mat = LineFunction(t,p0,p1,s) 2 | 3 | l = length(p0); 4 | for i=1:l 5 | mat(:,i) = [p0(i)+(p1(i)-p0(i))*t;(p1(i)-p0(i));0]; 6 | end 7 | 8 | end -------------------------------------------------------------------------------- /apps/trajectory_planning_under_actuated/NormalizedPoly7Coefficients.m: -------------------------------------------------------------------------------- 1 | function coef = NormalizedPoly7Coefficients(T,coef) 2 | %NORMALIZEDPOLY7COEFFICIENTS computes the coefficients of a normalized polynomial function of degree 7. 3 | % 4 | % NORMALIZEDPOLY7COEFFICIENTS computes the nonzero coefficients of a 5 | % normalized polynomial function of degree 7, so that in 0 and T the 6 | % function and its 1th,2nd and 3th order derivatives are equal to 0. 7 | % Moreover it returns the coefficients of the polynomial function and 8 | % its 1th and 2nd order derivatives. 9 | % 10 | % T is the instant time selected for polynomial boundary conditions. 11 | % COEF is a structure containing the coefficients of the polynomial 12 | % function and its derivatives. 13 | % 14 | % C is a vector (size[4,1]), field of the structure COEF containing the 15 | % nonzeros polynomial coefficients. 16 | % CDERIVATIVE is a vector (size[4,1]), field of the structure COEF 17 | % containing the nonzero 1th order derivative polynomial coefficients. 18 | % CDERIVATIVE2 is a vector (size[4,1]), field of the structure COEF 19 | % containing the nonzero 2nd order derivative polynomial coefficients. 20 | 21 | 22 | m=zeros(4); 23 | t=zeros(4); 24 | md=zeros(4); 25 | for i = 1:4 26 | for j = 1:4 27 | if i == 1 28 | m(i,j) = 1; 29 | else 30 | m(i,j) = m(i-1,j)*(j+5-i); 31 | end 32 | t(i,j) = T^(j-i+4); 33 | end 34 | md(i,:) = m(i,:).*t(i,:); 35 | end 36 | 37 | for i = 1:4 38 | 39 | end 40 | 41 | b = zeros(4,1); 42 | b(1) = 1; 43 | c = linsolve(md,b); 44 | 45 | coef.c = m(1,:)'.*c; 46 | coef.cDerivative = m(2,:)'.*c; 47 | coef.cDerivative2 = m(3,:)'.*c; 48 | 49 | end -------------------------------------------------------------------------------- /apps/trajectory_planning_under_actuated/NormalizedTime.m: -------------------------------------------------------------------------------- 1 | function v = NormalizedTime(k,t,T) 2 | 3 | x = t/T; 4 | 5 | h = k(1)+k(2)*t+k(3)*t^2+k(4)*t^3+k(5)*t^4+k(6)*t^5; 6 | hp = k(2)+2*k(3)*t+3*k(4)*t^2+4*k(5)*t^3+5*k(6)*t^4; 7 | hpp = 2*k(3)+6*k(4)*t+12*k(5)*t^2+20*k(6)*t^3; 8 | 9 | v(1) = x*(1+(1-x)*h); 10 | v(2) = x*(1-x)*hp+(1+(1-2*x)*h)/T; 11 | v(3) = x*(1-x)*hpp+2*((1-2*x)*hp-h/T)/T; 12 | 13 | end -------------------------------------------------------------------------------- /apps/trajectory_planning_under_actuated/RestToRestCoefficients.m: -------------------------------------------------------------------------------- 1 | function [output] = RestToRestCoefficients(cdpr_p,cdpr_v,... 2 | sim_data,geom_fun,ut,record) 3 | 4 | for i = 1:sim_data.pNumber-1 5 | coefficientGuess = zeros(2*(cdpr_p.pose_dim-cdpr_p.n_cables),1); 6 | [sim_data.coeff(:,i),fval,exitFlag1,Foutput] = fsolve(@(k) FindOptimalCoefficients(k,cdpr_p,cdpr_v,... 7 | ut,sim_data,i,geom_fun,record),coefficientGuess,ut.fsolve_options); 8 | out = GenerateOutputUnderActuated(i,cdpr_p,cdpr_v,sim_data,geom_fun,ut); 9 | output(i).fval = fval; 10 | output(i).t = out.t; 11 | output(i).platform = out.platform; 12 | output(i).cables(:) = out.cable(:); 13 | output(i).coefficients = sim_data.coeff(:,i); 14 | 15 | end 16 | 17 | end -------------------------------------------------------------------------------- /apps/trajectory_planning_under_actuated/StandardInverseSimulator.m: -------------------------------------------------------------------------------- 1 | function [output] = StandardInverseSimulator(cdpr_p,cdpr_v,... 2 | sim_data,geom_fun,ut) 3 | 4 | for i = 1:sim_data.pNumber-1 5 | 6 | sim_data.coeff(:,i) = zeros(6,1); 7 | out = GenerateOutputUnderActuated(i,cdpr_p,cdpr_v,sim_data,geom_fun,ut); 8 | output(i).t = out.t; 9 | output(i).platform = out.platform; 10 | output(i).cables(:) = out.cable(:); 11 | output(i).coefficients = sim_data.coeff(:,i); 12 | % for j=1:length(out.tension_vector) 13 | % cdpr_v = UpdateIKZeroOrd(output(i).platform.pose(1:3,j),output(i).platform.pose(4:6,j),cdpr_p,cdpr_v); 14 | % output(i).platform.twist(:,j) = [output(i).platform.pose_d(1:3,j);cdpr_v.platform.H_mat*output(i).platform.pose_d(4:6,j)]; 15 | % twist_non_act = [cdpr_v.geometric_jacobian(:,4:6)'*inv(cdpr_v.geometric_jacobian(:,1:3)') -eye(3)]'; 16 | % output(i).platform.twist_na(:,j) = zeros(6,1); 17 | % for k=1:3 18 | % magnitude(k) = output(i).platform.twist(:,j)'*twist_non_act(:,k)./(twist_non_act(:,k)'*twist_non_act(:,k)); 19 | % output(i).platform.twist_na(:,j) = output(i).platform.twist_na(:,j) + magnitude(k).*twist_non_act(:,k); 20 | % end 21 | % output(i).platform.twist_a(:,j) = output(i).platform.twist(:,j)-output(i).platform.twist_na(:,j); 22 | % output(i).platform.verify(:,j) = cdpr_v.geometric_jacobian*output(i).platform.twist_a(:,j); 23 | % end 24 | 25 | end 26 | 27 | end -------------------------------------------------------------------------------- /apps/trajectory_planning_under_actuated/TestTransitionPolynomials.m: -------------------------------------------------------------------------------- 1 | function c = TestTransitionPolynomials(degree) 2 | 3 | c = zeros(2*degree+1,1); 4 | c_d = zeros(2*degree+1,1); 5 | c_d_2 = zeros(2*degree+1,1); 6 | sum_c = 0; 7 | sum_c_d = 0; 8 | sum_c_d_2 = 0; 9 | for i=degree+1:2*degree+1 10 | c(i,1) = ((-1)^(i-degree-1)*factorial(2*degree+1))/... 11 | (i*factorial(degree)*factorial(i-degree-1)*factorial(2*degree+1-i)); 12 | c_d(i-1,1) = c(i,1)*i; 13 | c_d_2(i-2,1) = c_d(i-1,1)*(i-1); 14 | sum_c = sum_c+c(i,1); 15 | sum_c_d = sum_c_d+c_d(i-1,1); 16 | sum_c_d_2 = sum_c_d_2+c_d_2(i-2,1); 17 | end 18 | 19 | ind = 0; 20 | tVect = zeros(2*degree+1,1); 21 | for tt=0:0.01:1 22 | ind = ind+1; 23 | t(ind) = tt; 24 | for j=1:2*degree+1 25 | tVect(j) = tt^j; 26 | end 27 | f(ind) = dot(c,tVect); 28 | f_d(ind) = dot(c_d,tVect); 29 | f_d_2(ind) = dot(c_d_2,tVect); 30 | end 31 | 32 | hold on 33 | grid on 34 | plot(t,f/(max(abs(f)))) 35 | plot(t,f_d/(max(abs(f_d)))) 36 | plot(t,f_d_2/(max(abs(f_d_2)))) 37 | hold off 38 | end -------------------------------------------------------------------------------- /apps/workspace_computation/CalcCablesStaticTensionNoCheck.m: -------------------------------------------------------------------------------- 1 | function cdpr_v = CalcCablesStaticTensionNoCheck(cdpr_v) 2 | %CALCCABLESTENSION computes the cables tension under static equilibrium conditions. 3 | % 4 | % ANALITIC_JACOBIAN is the analitic jacobian matrix (size[nc,6]), where 5 | % nc is the number of cables. 6 | % EXT_LOAD is a vector(size[6,1]), containing the components of the 7 | % external forces and moments acting on the platform. 8 | % 9 | % VECTOR is a vector(size[nc,1]), containing the tension values acting 10 | % on each cable. 11 | % 12 | % CDPR_V is a structure containing time dependent variables of the cdpr. 13 | 14 | cdpr_v.tension_vector = linsolve(cdpr_v.geometric_jacobian*cdpr_v.geometric_jacobian',... 15 | cdpr_v.geometric_jacobian*cdpr_v.platform.ext_load); 16 | 17 | 18 | end -------------------------------------------------------------------------------- /apps/workspace_computation/CalcOrientationWorkSpace.m: -------------------------------------------------------------------------------- 1 | function out = CalcOrientationWorkSpace(cdpr_p,cdpr_v,tau_lim,position,varargin) 2 | 3 | ws_angle_step = pi/90; 4 | out.counter = 0; 5 | 6 | if (~isempty(varargin)) 7 | rec = varargin{1}; 8 | end 9 | 10 | 11 | 12 | tors = 0; 13 | for tilt = 0:ws_angle_step:pi/2-pi/18 14 | for ang = 0:ws_angle_step:2*pi-ws_angle_step 15 | ang_var = [ang;tilt;tors]; 16 | pose = [position;ang_var]; 17 | [out] = CheckPoseInOrientWorkSpace(cdpr_p,cdpr_v,tau_lim,out,pose); 18 | end 19 | end 20 | 21 | out.limits = CalcWorkspaceGraphicalLimits(cdpr_p,out); 22 | out.workspace_center = mean(out.limits,2); 23 | out.parametrization = cdpr_p.rotation_parametrization; 24 | 25 | end -------------------------------------------------------------------------------- /apps/workspace_computation/CalcWorkspace.m: -------------------------------------------------------------------------------- 1 | function out = CalcWorkspace(cdpr_p,cdpr_v,ut,out,type,tau_limits,folder,rec,varargin) 2 | 3 | n_arg_opt = length(varargin); 4 | 5 | if (n_arg_opt>0) 6 | add_wp_info = varargin{1}; 7 | end 8 | 9 | if (type == 1) % translational 10 | 11 | if (cdpr_p.n_cables <6) % underactuated 12 | 13 | out = CalcWorkspaceUnder33(cdpr_p,cdpr_v,ut,tau_limits,add_wp_info); 14 | rec = rec.ResetFigureLimits(out.limits,10); 15 | plot_handle = DisplayAndSaveWorkspace(out,0,folder,rec); 16 | delete(plot_handle); 17 | plot_handle = DisplayAndSaveWorkspace(out,1,folder,rec); 18 | delete(plot_handle); 19 | 20 | elseif (cdpr_p.n_cables >6) % overactuated 21 | 22 | else % completely actuated 23 | 24 | end 25 | 26 | else % orientational 27 | 28 | if (cdpr_p.n_cables <6) % underactuated 29 | 30 | elseif (cdpr_p.n_cables >6) % overactuated 31 | 32 | else % completely actuated 33 | out = CalcOrientationWorkSpace(cdpr_p,cdpr_v,tau_limits,add_wp_info); 34 | DisplayAndSaveWorkspace(out,2,folder,rec); 35 | end 36 | 37 | end 38 | 39 | end -------------------------------------------------------------------------------- /apps/workspace_computation/CalcWorkspaceGraphicalLimits.m: -------------------------------------------------------------------------------- 1 | function limits = CalcWorkspaceGraphicalLimits(cdpr_p,ws_data) 2 | 3 | lim_sup = [Inf;Inf;Inf]; 4 | lim_inf = -[Inf;Inf;Inf]; 5 | 6 | cdpr_v = CdprVar(cdpr_p.n_cables); 7 | for k = 1: length(ws_data) 8 | cdpr_v = UpdateIKZeroOrd(ws_data.pose(1:3,k),ws_data.pose(4:end,k),cdpr_p,cdpr_v); 9 | for i=1:cdpr_p.n_cables 10 | for l = 1:3 11 | switch l 12 | case 1 13 | point = cdpr_p.cable(i).pos_OD_glob; 14 | case 2 15 | point = cdpr_v.cable(i).pos_OA_glob; 16 | case 3 17 | point = cdpr_v.platform.position; 18 | end 19 | if (i==1 && l==1) 20 | lim_sup = point; 21 | lim_inf = lim_sup; 22 | 23 | else 24 | for j=1:3 25 | if (point(j)>lim_sup(j)) 26 | lim_sup(j) = point(j); 27 | elseif (point(j)limits(j,2)) 21 | limits(j,2) = point(j); 22 | elseif (point(j)tau_lim(2)))... 12 | && isempty(cdpr_v.tension_vector(cdpr_v.tension_vectortau_lim(2)))... 16 | && isempty(cdpr_v.tension_vector(cdpr_v.tension_vector= frame_counter*ut.sampling_count) 8 | frame_counter = frame_counter +1; 9 | var = UpdateIKZeroOrd(ss.platform.pose(1:3,j),ss.platform.pose(4:end,j),par,var); 10 | rec.SetFrame(var,par); 11 | frames(frame_counter) = getframe(rec.figure_handle); 12 | if (frame_counter == n_frames) 13 | break; 14 | end 15 | end 16 | end 17 | myVideo = VideoWriter(filename); 18 | myVideo.FrameRate = ut.frame_rate; 19 | open(myVideo) 20 | writeVideo(myVideo,frames); 21 | close(myVideo); 22 | clear frames 23 | end 24 | 25 | end -------------------------------------------------------------------------------- /libs/export_utilities/RecordType.m: -------------------------------------------------------------------------------- 1 | classdef RecordType 2 | properties 3 | n_cables; 4 | figure_handle; 5 | axes_handle; 6 | lines; 7 | frame_vector; 8 | end 9 | methods 10 | function obj = RecordType(par,title,name) 11 | obj.n_cables = par.n_cables; 12 | obj = obj.SetFigureParameter(par,title,name); 13 | for i=1:obj.n_cables 14 | obj.lines.pulley(i) = animatedline('Color','r','LineWidth',2); 15 | obj.lines.cable(i) = animatedline('Color','k','LineWidth',2); 16 | obj.lines.platformPoint(i) = animatedline('Color','b','LineWidth',1); 17 | obj.lines.platformEdge(i) = animatedline('Color','b','LineWidth',1); 18 | obj.lines.platformPlane(i) = animatedline('Color','b','LineWidth',1); 19 | end 20 | end 21 | function obj = SetFigureParameter(obj,par,title,name) 22 | obj.figure_handle = figure('Name',title,'FileName',name,'NumberTitle','off','Position',[0 0 700 700]); 23 | for i=1:par.n_cables 24 | if (norm(par.cable(i,1).pos_OD_glob)>norm(par.cable(i,1).pos_PA_loc)) 25 | limits(:,i) = par.cable(i,1).pos_OD_glob; 26 | else 27 | limits(:,i) = par.cable(i,1).pos_PA_loc; 28 | end 29 | end 30 | hold on 31 | max_x = max(limits(1,:)); min_x = min(limits(1,:)); incr_x = 0.2*(max_x - min_x); 32 | lim_x = [((min_x-incr_x)*10) ((max_x+incr_x)*10)]./10; l_x = (lim_x(2)-lim_x(1)); tickSpace_x = l_x/10; 33 | max_y = max(limits(2,:)); min_y = min(limits(2,:)); incr_y = 0.2*(max_y - min_y); 34 | lim_y = [((min_y-incr_y)*10) ((max_y+incr_y)*10)]./10; l_y = (lim_y(2)-lim_y(1)); tickSpace_y = l_y/10; 35 | max_z = max(limits(3,:)); min_z = min(limits(3,:)); middle_z = (max_z - min_z)/2; 36 | max_l = max([lim_x lim_y]); lim_z = [((middle_z-max_l)*15) (abs(max_z)*15)]./10; l_z = (lim_z(2)-lim_z(1)); 37 | tickSpace_z = l_z/10; 38 | 39 | obj.axes_handle = gca; obj.axes_handle.Box = 'Off'; obj.axes_handle.LineWidth = 2; 40 | obj.axes_handle.XAxisLocation = 'origin'; obj.axes_handle.XLim = lim_x; 41 | obj.axes_handle.XTick = lim_x(1):tickSpace_x:lim_x(2); obj.axes_handle.XGrid = 'On'; 42 | obj.axes_handle.YAxisLocation = 'origin'; obj.axes_handle.ZLim = lim_z; 43 | obj.axes_handle.ZTick = lim_z(1):tickSpace_z:lim_z(2); obj.axes_handle.YGrid = 'On'; 44 | obj.axes_handle.YLim = lim_y; obj.axes_handle.YTick = lim_y(1):tickSpace_y:lim_y(2); 45 | obj.axes_handle.ZGrid = 'On'; obj.axes_handle.GridLineStyle = '--'; 46 | 47 | obj.axes_handle.XLabel.String = 'x'; obj.axes_handle.XLabel.FontSize = 16; 48 | obj.axes_handle.XLabel.FontWeight = 'bold'; 49 | obj.axes_handle.YLabel.String = 'y'; obj.axes_handle.YLabel.FontSize = 16; 50 | obj.axes_handle.YLabel.Rotation = 0; obj.axes_handle.YLabel.FontWeight = 'bold'; 51 | obj.axes_handle.ZLabel.String = 'z'; obj.axes_handle.ZLabel.FontSize = 16; 52 | obj.axes_handle.ZLabel.Rotation = 0; obj.axes_handle.ZLabel.FontWeight = 'bold'; 53 | 54 | obj.axes_handle.DataAspectRatioMode = 'manual'; 55 | obj.axes_handle.DataAspectRatio= [1;1;1]; 56 | 57 | obj.axes_handle.CameraPosition = [-2.837916634960155,-22.911510361523757,13.494156116891816]; 58 | %obj.axes_handle.CameraTarget = [-0.913362548387552,0.616539044482742,-0.747766910811659]; 59 | %obj.axes_handle.CameraViewAngle = 9.3176; 60 | end 61 | function obj = ResetFigureLimits(obj,limits,spacing) 62 | 63 | obj.axes_handle.XLim = limits(1,:); 64 | obj.axes_handle.YLim = limits(2,:); 65 | obj.axes_handle.ZLim = limits(3,:); 66 | obj.axes_handle.XTick = limits(1,1):(limits(1,2)-limits(1,1))/spacing:limits(1,2); 67 | obj.axes_handle.YTick = limits(2,1):(limits(2,2)-limits(2,1))/spacing:limits(2,2); 68 | obj.axes_handle.ZTick = limits(3,1):(limits(3,2)-limits(3,1))/spacing:limits(3,2); 69 | 70 | obj.axes_handle.DataAspectRatioMode = 'manual'; 71 | obj.axes_handle.DataAspectRatio= [1;1;1]; 72 | 73 | obj.axes_handle.CameraPosition = [-2.837916634960155,-22.911510361523757,13.494156116891816]; 74 | 75 | end 76 | function frame = SetFrame(obj,cdpr_v,cdpr_p) 77 | for i=1:obj.n_cables 78 | clearpoints(obj.lines.pulley(i)); 79 | clearpoints(obj.lines.cable(i)); 80 | clearpoints(obj.lines.platformPoint(i)); 81 | clearpoints(obj.lines.platformEdge(i)); 82 | clearpoints(obj.lines.platformPlane(i)); 83 | k = cdpr_v.platform.rot_mat(:,3); 84 | r = cdpr_v.platform.position; 85 | a = cdpr_v.cable(i).pos_OA_glob; 86 | p(:,i) = RecordType.GetIntersectionInPlane(r,a,k); 87 | for j = 0:0.05:2*pi+0.05 88 | vers_n_rot = cdpr_v.cable(i).vers_u*cos(j) +... 89 | cdpr_p.cable(i).vers_k*sin(j); 90 | r = cdpr_p.cable(i).pos_OD_glob+cdpr_p.cable(i).swivel_pulley_r*... 91 | (cdpr_v.cable(i).vers_u+vers_n_rot); 92 | addpoints(obj.lines.pulley(i),[r(1)],[r(2)],[r(3)]); 93 | end 94 | addpoints(obj.lines.platformEdge(i),... 95 | [cdpr_v.cable(i).pos_OA_glob(1) cdpr_v.platform.position(1)],... 96 | [cdpr_v.cable(i).pos_OA_glob(2) cdpr_v.platform.position(2)],... 97 | [cdpr_v.cable(i).pos_OA_glob(3) cdpr_v.platform.position(3)]); 98 | addpoints(obj.lines.platformPoint(i),... 99 | [p(1,i) cdpr_v.platform.position(1)],... 100 | [p(2,i) cdpr_v.platform.position(2)],... 101 | [p(3,i) cdpr_v.platform.position(3)]); 102 | addpoints(obj.lines.platformPlane(i),... 103 | [cdpr_v.cable(i).pos_OA_glob(1) p(1,i)],... 104 | [cdpr_v.cable(i).pos_OA_glob(2) p(2,i)],... 105 | [cdpr_v.cable(i).pos_OA_glob(3) p(3,i)]); 106 | r = cdpr_p.cable(i).pos_OD_glob+cdpr_p.cable(i).swivel_pulley_r*... 107 | (cdpr_v.cable(i).vers_u+cdpr_v.cable(i).vers_n); 108 | addpoints(obj.lines.cable(i),... 109 | [cdpr_v.cable(i).pos_OA_glob(1) r(1,1)],... 110 | [cdpr_v.cable(i).pos_OA_glob(2) r(2,1)],... 111 | [cdpr_v.cable(i).pos_OA_glob(3) r(3,1)]); 112 | frame = getframe(obj.figure_handle); 113 | end 114 | end 115 | end 116 | methods (Static) 117 | function p = GetIntersectionInPlane(r,a,k) 118 | A = zeros(3); 119 | v = zeros(3,1); 120 | A(3,:) = k'; 121 | v(3) = k'*a; 122 | [~,index] = max(abs(k)); 123 | switch index 124 | case 1 125 | A(1:2,:) = [k(2) -k(1) 0; 126 | k(3) 0 -k(1)]; 127 | v(1:2,1) = [-k(1)*r(2)+k(2)*r(1);-k(1)*r(3)+k(3)*r(1)]; 128 | case 2 129 | A(1:2,:) = [k(2) -k(1) 0; 130 | 0 k(3) -k(2)]; 131 | v(1:2,1) = [-k(1)*r(2)+k(2)*r(1);-k(2)*r(3)+k(3)*r(2)]; 132 | case 3 133 | A(1:2,:) = [k(3) 0 -k(1) ; 134 | 0 k(3) -k(2)]; 135 | v(1:2,1) = [-k(1)*r(3)+k(3)*r(1);-k(2)*r(3)+k(3)*r(2)]; 136 | end 137 | p = linsolve(A,v); 138 | end 139 | end 140 | end -------------------------------------------------------------------------------- /libs/export_utilities/UtilitiesType.m: -------------------------------------------------------------------------------- 1 | classdef UtilitiesType 2 | properties 3 | data_logger_i = 1; 4 | frame_i = 1; 5 | t_interval = 0.001; 6 | frame_rate = 60; 7 | sampling_count = 1/(60*0.001); 8 | error = 0; 9 | cutt_off_freq = 50; 10 | baum_zita = 10; 11 | baum_omega = 100; 12 | ode45_options = odeset('RelTol',1e-6,'AbsTol',1e-8); 13 | fsolve_options = optimoptions('fsolve','Algorithm',... 14 | 'levenberg-marquardt','FunctionTolerance',1e-8,'MaxFunctionEvaluation',... 15 | 1000000,'MaxIterations',1000000,'OptimalityTolerance',1e-1,... 16 | 'StepTolerance',1e-8,'UseParallel',true); 17 | fsolve_options_grad = optimoptions('fsolve','Algorithm',... 18 | 'levenberg-marquardt','FunctionTolerance',1e-8,'MaxFunctionEvaluation',... 19 | 1000000,'MaxIterations',1000000,'OptimalityTolerance',1e-8,... 20 | 'display','none','StepTolerance',1e-8,'SpecifyObjectiveGradient',true,'UseParallel',true); 21 | % fsolve_options_grad = optimoptions('fsolve','Algorithm',... 22 | % 'levenberg-marquardt','FunctionTolerance',1e-8,'MaxFunctionEvaluation',... 23 | % 1000000,'MaxIterations',1000000,'OptimalityTolerance',1e-8,... 24 | % 'StepTolerance',1e-8,'SpecifyObjectiveGradient',true,'UseParallel',true); 25 | lsqnonlin_options = optimoptions('lsqnonlin','Algorithm',... 26 | 'trust-region-reflective','FunctionTolerance',1e-6,'MaxFunctionEvaluation',... 27 | 1000000,'MaxIterations',1000000,'OptimalityTolerance',1e-6,'StepTolerance',1e-6,'UseParallel',true); 28 | % lsqnonlin_options = optimoptions('lsqnonlin','Algorithm',... 29 | % 'levenberg-marquardt','FunctionTolerance',1e-6,'MaxFunctionEvaluation',... 30 | % 1000000,'MaxIterations',1000000,'OptimalityTolerance',1e-6,'StepTolerance',1e-6,'UseParallel',true); 31 | lsqnonlin_options_grad = optimoptions('lsqnonlin','Algorithm',... 32 | 'levenberg-marquardt','FunctionTolerance',1e-6,'MaxFunctionEvaluation',... 33 | 1000000,'MaxIterations',1000000,'OptimalityTolerance',1e-6,... 34 | 'StepTolerance',1e-6,'SpecifyObjectiveGradient',true); 35 | fmincon_options = optimoptions('fmincon','Algorithm','interior-point',... 36 | 'FunctionTolerance',1e-8,'MaxFunctionEvaluation',1000000,... 37 | 'MaxIterations',1000000,'OptimalityTolerance',1e-8,'StepTolerance',1e-8); 38 | huen_c = [0;1]; 39 | huen_b = [1/2;1/2]; 40 | huen_M = [0 0 ; 41 | 1 0]; 42 | end 43 | methods 44 | function obj = ResetDataLoggerIndex(obj) 45 | obj.data_logger_i = 1; 46 | end 47 | function obj = ResetFrameIndex(obj) 48 | obj.frame_i = 1; 49 | end 50 | function obj = SetTimeInterval(obj,t) 51 | obj.t_interval = t; 52 | end 53 | function obj = SetFrameRate(obj,frame_r) 54 | obj.frame_rate = frame_r; 55 | if obj.t_interval > 0 56 | obj.sampling_count = 1/(obj.t_interval*obj.frame_rate); 57 | end 58 | end 59 | end 60 | end -------------------------------------------------------------------------------- /libs/numeric/Anti.m: -------------------------------------------------------------------------------- 1 | function mat = Anti(v) 2 | %ANTI computes the skew-symmetric matrix related to the array v. 3 | % The matrix is defined as follows in order to perform cross-products 4 | % in matrix form. 5 | % 6 | % V (size[3,1]) is the array of coordinates. 7 | % 8 | % MAT (size[3,3]) is the resulting skew-symmetric matrix. 9 | 10 | 11 | mat = [0 -v(3) v(2); 12 | v(3) 0 -v(1); 13 | -v(2) v(1) 0]; 14 | 15 | end -------------------------------------------------------------------------------- /libs/numeric/FiniteDifferentiation.m: -------------------------------------------------------------------------------- 1 | function der = FiniteDifferentiation(val,val_prev,diff_prev,t,ut) 2 | 3 | der = (val-val_prev)/ut.t_interval; 4 | der = MyLowPassFilt(der,diff_prev,t,ut); 5 | 6 | end -------------------------------------------------------------------------------- /libs/numeric/HuenDiscreteSolver.m: -------------------------------------------------------------------------------- 1 | function sol = HuenDiscreteSolver(f,t,p0) 2 | 3 | n = length(t); 4 | dim = length(p0); 5 | 6 | h = t(2)-t(1); 7 | 8 | sol.y = zeros(dim,n); 9 | sol.f = zeros(1,n); 10 | 11 | sol.t = t; 12 | sol.y(:,1) = p0; 13 | app = f(t(1),p0); 14 | sol.f(1:dim/2,1) = app(dim/2+1:dim,1); 15 | 16 | 17 | for i=2:n 18 | 19 | y_tilde = sol.y(:,i-1) + h.*[sol.y(end/2+1:end,i-1);sol.f(:,i-1)]; 20 | sol.y(:,i) = sol.y(:,i-1) + h./2.*([sol.y(end/2+1:end,i-1);sol.f(:,i-1)]... 21 | +f(t(i),y_tilde)); 22 | app = f(t(i),sol.y(:,i)); 23 | sol.f(1:dim/2,i) = app(dim/2+1:dim,1); 24 | 25 | end 26 | 27 | end -------------------------------------------------------------------------------- /libs/numeric/HuenSolver.m: -------------------------------------------------------------------------------- 1 | function sol = HuenSolver(f,t,p0) 2 | 3 | n = length(t); 4 | sol.t = t; 5 | dim = length(p0); 6 | sol.y = zeros(dim,n); 7 | sol.f = zeros(1,n); 8 | sol.y(:,1) = p0; 9 | app = f(t(1),p0); 10 | sol.f(1:dim/2,1) = app(dim/2+1:dim,1); 11 | h = t(2)-t(1); 12 | 13 | c = [0;2/3]; 14 | b = [1/4;3/4]; 15 | M = [0 0 ; 16 | 2/3 0]; 17 | K = zeros(dim,2); 18 | 19 | for i=2:n 20 | 21 | F = zeros(dim,1); 22 | 23 | for j = 1:2 24 | 25 | s = zeros(dim,1); 26 | for k = 1:j-1 27 | 28 | s = s + M(j,k).*K(:,k); 29 | 30 | end 31 | 32 | K(:,j) = f(t(i-1)+h*c(j),sol.y(:,i-1)+h.*s); 33 | 34 | F = F + b(j).*K(:,j); 35 | 36 | end 37 | 38 | sol.y(:,i) = sol.y(:,i-1) + h.*F; 39 | app = f(t(i),sol.y(:,i)); 40 | sol.f(1:dim/2,i) = app(dim/2+1:dim,1); 41 | 42 | end 43 | 44 | end -------------------------------------------------------------------------------- /libs/numeric/Interpolate.m: -------------------------------------------------------------------------------- 1 | function [pos,vel,acc] = Interpolate(sp,time,dt) 2 | 3 | pos = ppval(sp,time); 4 | 5 | if (time>=2*dt) 6 | pm1 = ppval(sp,time-dt); 7 | pm2 = ppval(sp,time-2.*dt); 8 | M = [1 0 0 9 | 1 (dt) (dt)^2; 10 | 1 (2*dt) (2*dt)^2]; 11 | F = [pm2;pm1;pos]; 12 | c = linsolve(M,F); 13 | vel = [0 1 2*(2*dt)]*c; 14 | acc = [0 0 2]*c; 15 | else 16 | if (time>=dt) 17 | pm1 = ppval(sp,time-dt); 18 | vel = (pos-pm1)/dt; 19 | acc = 0; 20 | else 21 | vel = 0; 22 | acc = 0; 23 | end 24 | end 25 | 26 | 27 | 28 | end -------------------------------------------------------------------------------- /libs/numeric/MyInv.m: -------------------------------------------------------------------------------- 1 | function mat = MyInv(m) 2 | 3 | dim = size(m); 4 | if (dim(1)~=dim(2)) 5 | m = m'*m; 6 | end 7 | dim = size(m); 8 | for i=1:dim(1) 9 | v = zeros(dim(1),1); 10 | v(i) = 1; 11 | mat(i,:) = linsolve(m,v); 12 | end 13 | 14 | end -------------------------------------------------------------------------------- /libs/numeric/MyLowPassFilt.m: -------------------------------------------------------------------------------- 1 | function now_filt_value = MyLowPassFilt(now_raw_value,prev_filt_value,t,ut) 2 | 3 | alpha = ut.cutt_off_freq * ut.t_interval; 4 | if (t > 0) 5 | now_filt_value = prev_filt_value + alpha * (now_raw_value - prev_filt_value); 6 | else 7 | now_filt_value = now_raw_value; 8 | end 9 | 10 | end -------------------------------------------------------------------------------- /libs/numeric/RKSolver.m: -------------------------------------------------------------------------------- 1 | function sol = RKSolver(f,t,p0) 2 | 3 | n = length(t); 4 | sol.t = t; 5 | dim = length(p0); 6 | sol.y = zeros(dim,n); 7 | sol.f = zeros(1,n); 8 | sol.y(:,1) = p0; 9 | app = f(t(1),p0); 10 | sol.f(1:dim/2,1) = app(dim/2+1:dim,1); 11 | h = t(2)-t(1); 12 | 13 | c = [0;1/4;3/8;12/13;1;1/2]; 14 | %b = [25/216;0;1408/2565;2197/4104;-1/5;0]; 15 | b = [16/135;0;6656/12825;28561/56430;-9/50;2/55]; 16 | M = [0 0 0 0 0 0; 17 | 1/4 0 0 0 0 0; 18 | 3/32 9/32 0 0 0 0; 19 | 1932/2197 -7200/2197 7296/2197 0 0 0; 20 | 439/216 -8 3680/513 -845/4104 0 0; 21 | -8/27 2 -3544/2565 1859/4104 -11/40 0]; 22 | K = zeros(dim,6); 23 | 24 | for i=2:n 25 | 26 | F = zeros(dim,1); 27 | 28 | for j = 1:6 29 | 30 | s = zeros(dim,1); 31 | for k = 1:j-1 32 | 33 | s = s + M(j,k).*K(:,k); 34 | 35 | end 36 | 37 | K(:,j) = f(t(i-1)+h*c(j),sol.y(:,i-1)+h.*s); 38 | 39 | F = F + b(j).*K(:,j); 40 | 41 | end 42 | 43 | sol.y(:,i) = sol.y(:,i-1) + h.*F; 44 | app = f(t(i),sol.y(:,i)); 45 | sol.f(1:dim/2,i) = app(dim/2+1:dim,1); 46 | 47 | end 48 | 49 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/DHtfQuaternion.m: -------------------------------------------------------------------------------- 1 | function mat = DHtfQuaternion(dq) 2 | %DHTFQUATERNION computes the 1th order time derivative of the matrix HTFQUATERNION. 3 | % 4 | % DQ (size[4,1]) is the 1th order time derivatives of quaternion components. 5 | % 6 | % MAT (size[3,4]) is the resulting transformation matrix. 7 | 8 | mat(1,1) = -dq(2); 9 | mat(1,2) = dq(1); 10 | mat(1,3) = -dq(4); 11 | mat(1,4) = dq(3); 12 | 13 | mat(2,1) = -dq(3); 14 | mat(2,2) = dq(4); 15 | mat(2,3) = dq(1); 16 | mat(2,4) = -dq(2); 17 | 18 | mat(3,1) = -dq(4); 19 | mat(3,2) = -dq(3); 20 | mat(3,3) = dq(2); 21 | mat(3,4) = dq(1); 22 | 23 | mat=2*mat; 24 | 25 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/DHtfRPY.m: -------------------------------------------------------------------------------- 1 | function mat = DHtfRPY(angle,angle_d) 2 | %DHTFRPY computes the 1th order time derivative of the matrix HTFRPY. 3 | % 4 | % ANGLE is the vector (size[3,1], [rad]) containing the rotation angles. 5 | % ANGLE_D is the vector (size[3,1], [rad]) containing the 1th order 6 | % time derivatives of rotation angles. 7 | % 8 | % MAT (size[3,3]) is the resulting transformation matrix. 9 | 10 | c1 = cos(angle(1)); s1 = sin(angle(1)); 11 | c2 = cos(angle(2)); s2 = sin(angle(2)); 12 | mat = zeros(3); 13 | mat(1,2) = -c1*angle_d(1); 14 | mat(1,3) = -s1*c2*angle_d(1) - c1*s2*angle_d(2); 15 | mat(2,2) = -s1*angle_d(1); 16 | mat(2,3) = c1*c2*angle_d(1) - s1*s2*angle_d(2); 17 | mat(3,3) = -c2*angle_d(2); 18 | 19 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/DHtfTaytBryan.m: -------------------------------------------------------------------------------- 1 | function mat = DHtfTaytBryan(angle,angle_d) 2 | %DHTFTAYTBRYAN computes the 1th order time derivative of the matrix HTFTAYTBRYAN. 3 | % 4 | % ANGLE is the vector (size[3,1], [rad]) containing the rotation angles. 5 | % ANGLE_D is the vector (size[3,1], [rad]) containing the 1th order 6 | % time derivatives of rotation angles. 7 | % 8 | % MAT (size[3,3]) is the resulting transformation matrix. 9 | 10 | c1 = cos(angle(1)); s1 = sin(angle(1)); 11 | c2 = cos(angle(2)); s2 = sin(angle(2)); 12 | mat = zeros(3); 13 | mat(1,3) = c2*angle_d(2); 14 | mat(2,2) = -s1*angle_d(1); 15 | mat(2,3) = -c1*c2*angle_d(1)+s1*s2*angle_d(2); 16 | mat(3,2) = c1*angle_d(1); 17 | mat(3,3) = -s1*c2*angle_d(1)-c1*s2*angle_d(2); 18 | 19 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/DHtfTiltTorsion.m: -------------------------------------------------------------------------------- 1 | function mat = DHtfTiltTorsion(angle,angle_d) 2 | %DHTFTILTTORSION computes the 1th order time derivative of the matrix HTFTILTTORSION. 3 | % 4 | % ANGLE is the vector (size[3,1], [rad]) containing the rotation angles. 5 | % ANGLE_D is the vector (size[3,1], [rad]) containing the 1th order 6 | % time derivatives of rotation angles. 7 | % 8 | % MAT (size[3,3]) is the resulting transformation matrix. 9 | 10 | c1 = cos(angle(1)); s1 = sin(angle(1)); 11 | c2 = cos(angle(2)); s2 = sin(angle(2)); 12 | mat(1,1) = s1*s2*angle_d(1)-c1*c2*angle_d(2); 13 | mat(1,2) = -c1*angle_d(1); 14 | mat(1,3) = -mat(1,1); 15 | mat(2,1) = -c1*s2*angle_d(1)-s1*c2*angle_d(2); 16 | mat(2,2) = -s1*angle_d(1); 17 | mat(2,3) = -mat(2,1); 18 | mat(3,1) = s2*angle_d(2); 19 | mat(3,2) = 0; 20 | mat(3,3) = -mat(3,1); 21 | 22 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/DHtfZYZ.m: -------------------------------------------------------------------------------- 1 | function mat = DHtfZYZ(angle,angle_d) 2 | %DHTFZYZ computes the 1th order time derivative of the matrix HTFZYZ. 3 | % 4 | % ANGLE is the vector (size[3,1], [rad]) containing the Euler angles. 5 | % ANGLE_D is the vector (size[3,1], [rad]) containing the 1th order 6 | % time derivatives of Euler angles. 7 | % 8 | % MAT (size[3,3]) is the resulting transformation matrix. 9 | 10 | c1 = cos(angle(1)); s1 = sin(angle(1)); 11 | c2 = cos(angle(2)); s2 = sin(angle(2)); 12 | mat = zeros(3); 13 | mat(1,2) = -c1*angle_d(1); 14 | mat(1,3) = -s1*s2*angle_d(1) + c1*c2*angle_d(2); 15 | mat(2,2) = -s1*angle_d(1); 16 | mat(2,3) = c1*s2*angle_d(1) + s1*c2*angle_d(2); 17 | mat(3,3) = -s2*angle_d(2); 18 | 19 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/HtfQuaternion.m: -------------------------------------------------------------------------------- 1 | function mat = HtfQuaternion(q) 2 | %HTFQUATERNION relates the 1th order derivatives of quaternion to angular velocity. 3 | % HTFQUATERNION computes the matrix that transforms the 1th order 4 | % time derivatives of quaternion components into the vector angular 5 | % velocity, projected in the fixed frame. 6 | % 7 | % Q is the quaternion. 8 | % 9 | % MAT (size[3,4]) is the transformation matrix. 10 | 11 | 12 | mat(1,1) = -q(2); 13 | mat(1,2) = q(1); 14 | mat(1,3) = -q(4); 15 | mat(1,4) = q(3); 16 | 17 | mat(2,1) = -q(3); 18 | mat(2,2) = q(4); 19 | mat(2,3) = q(1); 20 | mat(2,4) = -q(2); 21 | 22 | mat(3,1) = -q(4); 23 | mat(3,2) = -q(3); 24 | mat(3,3) = q(2); 25 | mat(3,4) = q(1); 26 | 27 | mat=2*mat; 28 | 29 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/HtfRPY.m: -------------------------------------------------------------------------------- 1 | function mat = HtfRPY(angle) 2 | %HTFRPY relates the 1th order derivatives of rotation angles to angular velocity. 3 | % HTFRPY computes the matrix that transforms the 1th order time 4 | % derivatives of rotation angles into the components of the vector 5 | % angular velocity, projected in the fixed frame. The angles of 6 | % rotation are chosen according to Roll Pitch Yaw parameterization. 7 | % 8 | % ANGLE is the vector (size[3,1], [rad]) containing the angles of rotation. 9 | % 10 | % MAT (size[3,3]) is the transformation matrix. 11 | 12 | 13 | c1 = cos(angle(1)); s1 = sin(angle(1)); 14 | c2 = cos(angle(2)); s2 = sin(angle(2)); 15 | mat = zeros(3); 16 | mat(1,2) = -s1; 17 | mat(1,3) = c1*c2; 18 | mat(2,2) = c1; 19 | mat(2,3) = s1*c2; 20 | mat(3,1) = 1; 21 | mat(3,3) = -s2; 22 | 23 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/HtfTaytBryan.m: -------------------------------------------------------------------------------- 1 | function mat = HtfTaytBryan(angle) 2 | %HTFTAYTBRYAN relates the 1th order derivatives of rotation angles to angular velocity. 3 | % HTFTAYTBRYAN computes the matrix that transforms the 1th order 4 | % time derivatives of rotation angles into the components of the 5 | % vector angular velocity, projected in the fixed frame. The angles 6 | % of rotation are chosen according to Tayt Bryan parameterization. 7 | % 8 | % ANGLE is the vector (size[3,1], [rad]) containing the angles of rotation. 9 | % 10 | % MAT (size[3,3]) is the transformation matrix. 11 | 12 | 13 | c1 = cos(angle(1)); s1 = sin(angle(1)); 14 | c2 = cos(angle(2)); s2 = sin(angle(2)); 15 | mat = eye(3); 16 | mat(1,3) = s2; 17 | mat(2,2) = c1; 18 | mat(2,3) = -s1*c2; 19 | mat(3,2) = s1; 20 | mat(3,3) = c1*c2; 21 | 22 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/HtfTiltTorsion.m: -------------------------------------------------------------------------------- 1 | function mat = HtfTiltTorsion(angle) 2 | %HTFTILTTORSION relates the 1th order derivatives of rotation angles to angular velocity. 3 | % HTFTILTTORSION computes the matrix that transforms the 1th order 4 | % time derivatives of rotation angles into the components of the vector 5 | % angular velocity, projected in the fixed frame. The angles of rotation 6 | % are chosen according to Tilt and Torsion parameterization. 7 | % 8 | % ANGLE is the vector (size[3,1], [rad]) containing the angles of rotation. 9 | % 10 | % MAT (size[3,3]) is the transformation matrix. 11 | 12 | c1 = cos(angle(1)); s1 = sin(angle(1)); 13 | c2 = cos(angle(2)); s2 = sin(angle(2)); 14 | mat = zeros(3); 15 | mat(1,1) = -c1*s2; 16 | mat(1,2) = -s1; 17 | mat(1,3) = -mat(1,1); 18 | mat(2,1) = -s1*s2; 19 | mat(2,2) = c1; 20 | mat(2,3) = -mat(2,1); 21 | mat(3,1) = 1-c2; 22 | mat(3,3) = c2; 23 | 24 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/HtfZYZ.m: -------------------------------------------------------------------------------- 1 | function mat = HtfZYZ(angle) 2 | %HTFZYZ relates the 1th order Euler angles derivatives to angular velocity. 3 | % HTFZYZ computes the matrix that transforms Euler angles 1th order 4 | % time derivatives into the components of the vector angular velocity, 5 | % projected in the fixed frame. 6 | % 7 | % ANGLE is the vector (size[3,1], [rad]) containing the Euler angles. 8 | % 9 | % MAT (size[3,3]) is the transformation matrix. 10 | 11 | c1 = cos(angle(1)); s1 = sin(angle(1)); 12 | c2 = cos(angle(2)); s2 = sin(angle(2)); 13 | mat = zeros(3); 14 | mat(1,2) = -s1; 15 | mat(1,3) = c1*s2; 16 | mat(2,2) = c1; 17 | mat(2,3) = s1*s2; 18 | mat(3,1) = 1; 19 | mat(3,3) = c2; 20 | 21 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/Quat2Rot.m: -------------------------------------------------------------------------------- 1 | function R = Quat2Rot(q) 2 | %QUAT2ROT computes the rotation matrix R, knowing the quaternion q. 3 | % 4 | % 5 | % Q is the quaternion. 6 | % 7 | % R is the resulting rotation matrix. 8 | R = zeros(3); 9 | R(1, 1) = q(1) * q(1) + q(2) * q(2) - q(3) * q(3) - q(4) * q(4); 10 | R(1, 2) = 2 * (q(2) * q(3) - q(1) * q(4)); 11 | R(1, 3) = 2 * (q(2) * q(4) + q(1) * q(3)); 12 | R(2, 1) = 2 * (q(2) * q(3) + q(1) * q(4)); 13 | R(2, 2) = q(1) * q(1) - q(2) * q(2) + q(3) * q(3) - q(4) * q(4); 14 | R(2, 3) = 2 * (q(3) * q(4) - q(1) * q(2)); 15 | R(3, 1) = 2 * (q(2) * q(4) - q(1) * q(3)); 16 | R(3, 2) = 2 * (q(3) * q(4) + q(1) * q(2)); 17 | R(3, 3) = q(1) * q(1) - q(2) * q(2) - q(3) * q(3) + q(4) * q(4); 18 | 19 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/Rot2Quat.m: -------------------------------------------------------------------------------- 1 | function q = Rot2Quat(R) 2 | %ROT2QUAT computes the quaternion components, knowing the rotation matrix R. 3 | % 4 | % The algorithm computes the quadratic value of each quaternion 5 | % components, arranged in Q_SQUARE. Quaternion theory ensures 6 | % Q_SQUARE maximum component is a nonzero value, thus it is 7 | % possibile to divide proper expressions by it in order to 8 | % evaluate the remaining components. Aforementioned expressions 9 | % involve rotation matrix elements and change according to the 10 | % position of the maximum value in the Q_SQUARE array. 11 | % 12 | % R is the rotation matrix. 13 | % 14 | % Q is the resulting quaternion. 15 | 16 | q_square(1) = 0.25 * (1 + R(1,1) + R(2,2) + R(3,3)); %=q1^2 17 | q_square(2) = 0.25 * (1 + R(1,1) - R(2,2) - R(3,3)); %=q2^2 18 | q_square(3) = 0.25 * (1 - R(1,1) + R(2,2) - R(3,3)); %=q3^2 19 | q_square(4) = 0.25 * (1 - R(1,1) - R(2,2) + R(3,3)); %=q4^2 20 | [~,i] = max(q_square); 21 | 22 | switch (i) 23 | case 1 24 | q(1,1) = sqrt(q_square(1)); 25 | q(2,1) = 0.25 * (R(3,2) - R(2,3)) / q(1); 26 | q(3,1) = 0.25 * (R(1,3) - R(3,1)) / q(1); 27 | q(4,1) = 0.25 * (R(2,1) - R(1,2)) / q(1); 28 | case 2 29 | q(2,1) = sqrt(q_square(2)); 30 | q(1,1) = 0.25 * (R(3,2) - R(2,3)) / q(2); 31 | q(3,1) = 0.25 * (R(1,2) + R(2,1)) / q(2); 32 | q(4,1) = 0.25 * (R(1,3) - R(3,1)) / q(2); 33 | case 3 34 | q(3,1) = sqrt(q_square(3)); 35 | q(1,1) = 0.25 * (R(1,3) - R(3,1)) / q(3); 36 | q(2,1) = 0.25 * (R(1,2) + R(2,1)) / q(3); 37 | q(4,1) = 0.25 * (R(2,3) + R(3,2)) / q(3); 38 | case 4 39 | q(4,1) = sqrt(q_square(4)); 40 | q(1,1) = 0.25 * (R(2,1) - R(1,2)) / q(4); 41 | q(2,1) = 0.25 * (R(1,3) + R(3,1)) / q(4); 42 | q(3,1) = 0.25 * (R(2,3) + R(3,2)) / q(4); 43 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/Rot2RPY.m: -------------------------------------------------------------------------------- 1 | function angle = Rot2RPY(R,aPrev) 2 | %ROT2RPY computes the magnitude of the rotations that result the matrix R. 3 | % 4 | % Moreover the three successive rotations are performed in the 5 | % following order: 6 | % 1) Rotation of magnitude V(1) about z axis of the moving frame. 7 | % 2) Rotation of magnitude V(2) about y axis of the moving frame. 8 | % 3) Rotation of magnitude V(3) about x axis of the moving frame. 9 | % 10 | % R is the rotation matrix. 11 | % APREV is the vector (size[3,1], [rad]) containing the magnitude of the 12 | % three successive rotations, referred to the previous instant time. 13 | % 14 | % V is the vector (size[3,1], [rad]) containing the magnitude of the three 15 | % successive rotations, respectively known as "yaw","pitch","roll". 16 | 17 | angle(2,1) = atan2(-R(3,1),(sqrt(R(3,2)^2+R(3,3)^2)+sqrt(R(1,1)^2+R(2,1)^2))/2); 18 | 19 | if cos(angle(2,1)) >= 0.0001 || cos(angle(2,1)) <= -0.0001 20 | angle(1,1) = atan2(R(3,2),R(3,3)); 21 | angle(3,1) = atan2(R(2,1),R(1,1)); 22 | else 23 | % Singularity of parametrization: cos(pitch)=0 24 | diff = atan2((-R(1,2)+R(2,3))/2,(R(1,3)+R(2,2))/2); 25 | angle(3,1) = aPrev(3); 26 | angle(1,1) = diff + angle(3); 27 | end 28 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/Rot2Tayt.m: -------------------------------------------------------------------------------- 1 | function angle = Rot2Tayt(R,aPrev) 2 | %ROT2TAYT 3 | 4 | angle(2,1) = atan2(R(1,3),(sqrt(R(1,1)^2+R(1,2)^2)+sqrt(R(2,3)^2+R(3,3)^2))/2); 5 | 6 | if cos(angle(2,1)) >= 0.0001 || cos(angle(2,1)) <= -0.0001 7 | angle(1,1) = atan2(-R(2,3),R(3,3)); 8 | angle(3,1) = atan2(-R(1,2),R(1,1)); 9 | else 10 | % Todo 11 | end 12 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/RotRPY.m: -------------------------------------------------------------------------------- 1 | function R = RotRPY(v) 2 | %ROTRPY computes the rotation matrix R of a sequence of three successive rotations about the moving frame. 3 | % Rotations are performed in the following order: 4 | % 1) Rotation of magnitude V(1) about z axis of the moving frame. 5 | % 2) Rotation of magnitude V(2) about y axis of the moving frame. 6 | % 3) Rotation of magnitude V(3) about x axis of the moving frame. 7 | % 8 | % V is a vector (size[3,1], [rad]) containing the magnitude of the three 9 | % successive rotations, respectively known as "yaw","pitch","roll". 10 | % 11 | % R is the overall rotation matrix. 12 | 13 | R = RotZ(v(1))*RotY(v(2))*RotX(v(3)); 14 | 15 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/RotTiltTorsion.m: -------------------------------------------------------------------------------- 1 | function R = RotTiltTorsion(v) 2 | %ROTTILTTORSION computes the rotation matrix R of a sequence of two successive rotations about body fixed axes. 3 | % 4 | % The two successive rotations about a and z axes of the moving frame of 5 | % magnitude V(2) and V(3), are respectively known as "Tilt" and "Torsion". 6 | % The axis a lies on the Oxy plane and its angular distances from y and x 7 | % axes are respectively equal to V(1) and V(1)+pi/2. The angle V(1) is 8 | % known as "azimuth". 9 | % The transformation can be thought of as a sequence of three 10 | % successive rotations, performed in the following order: 11 | % 1) Rotation of magnitude V(1) about z axis of the moving frame. 12 | % 2) Rotation of magnitude V(2) about y axis of the moving frame. 13 | % 3) Rotation of magnitude V(3)-V(1) about z axis of the moving frame. 14 | % 15 | % V is a vector (size[3,1], [rad]) containing the magnitude of the three 16 | % successive rotations. 17 | % 18 | % R is the overall rotation matrix. 19 | 20 | 21 | R = RotZYZ([v(1);v(2);v(3)-v(1)]); 22 | 23 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/RotX.m: -------------------------------------------------------------------------------- 1 | function R = RotX(angle) 2 | %ROTX computes the rotation matrix about world-frame X axis. 3 | % 4 | % ROTX computes the elementary rotation matrix R of 5 | % magnitude ANGLE about the fixed X axis of the world frame. 6 | % 7 | % ANGLE [rad] is the magnitude of rotation. 8 | % 9 | % R is the rotation matrix. 10 | 11 | 12 | R = eye(3); 13 | R(2,2) = cos(angle); 14 | R(3,3) = cos(angle); 15 | R(2,3) = -sin(angle); 16 | R(3,2) = sin(angle); 17 | 18 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/RotXYZ.m: -------------------------------------------------------------------------------- 1 | function R = RotXYZ(v) 2 | %ROTXYZ computes the rotation matrix R of a sequence of three successive rotations about the fixed frame. 3 | % 4 | % ROTXYZ computes the rotation matrix R of a sequence of three 5 | % successive rotations, performed in the following order: 6 | % 1) Rotation of magnitude v(3) about Z axis of the fixed frame 7 | % 2) Rotation of magnitude v(2) about Y axis of the fixed frame 8 | % 3) Rotation of magnitude v(1) about X axis of the fixed frame 9 | % 10 | % V is a vector (size[3,1], [rad]) containing the magnitude of the three 11 | % successive rotations. 12 | % 13 | % R is the overall rotation matrix. 14 | 15 | R = RotX(v(1))*RotY(v(2))*RotZ(v(3)); 16 | 17 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/RotY.m: -------------------------------------------------------------------------------- 1 | function R = RotY(angle) 2 | %ROTY computes the rotation matrix about world-frame Y axis. 3 | % 4 | % ROTY computes the elementary rotation matrix R of 5 | % magnitude ANGLE about the fixed Y axis of the world frame. 6 | % 7 | % ANGLE [rad] is the magnitude of rotation. 8 | % 9 | % R is the rotation matrix. 10 | 11 | R = eye(3); 12 | R(1,1) = cos(angle); 13 | R(3,3) = cos(angle); 14 | R(1,3) = sin(angle); 15 | R(3,1) = -sin(angle); 16 | 17 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/RotZ.m: -------------------------------------------------------------------------------- 1 | function R = RotZ(angle) 2 | %ROTZ computes the rotation matrix about world-frame Z axis. 3 | % 4 | % ROTZ computes the elementary rotation matrix R of 5 | % magnitude ANGLE about the fixed Z axis of the world frame. 6 | % 7 | % ANGLE [rad] is the magnitude of rotation. 8 | % 9 | % R is the rotation matrix. 10 | 11 | R = eye(3); 12 | R(1,1) = cos(angle); 13 | R(2,2) = cos(angle); 14 | R(1,2) = -sin(angle); 15 | R(2,1) = sin(angle); 16 | 17 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/RotZYZ.m: -------------------------------------------------------------------------------- 1 | function R = RotZYZ(v) 2 | %ROTZYZ computes the rotation matrix R of a sequence of three successive rotations about the moving frame. 3 | % 4 | % Rotations are performed in the following order: 5 | % 1) Rotation of magnitude V(1) about z axis of the moving frame. 6 | % 2) Rotation of magnitude V(2) about y axis of the moving frame. 7 | % 3) Rotation of magnitude V(3) about z axis of the moving frame. 8 | % 9 | % V is a vector (size[3,1], [rad]) containing the magnitude of the three 10 | % successive rotations, known as "Euler angles". 11 | % 12 | % R is the overall rotation matrix. 13 | 14 | R = RotZ(v(1))*RotY(v(2))*RotZ(v(3)); 15 | 16 | end -------------------------------------------------------------------------------- /libs/orientation_geometry/RotationParametrizations.m: -------------------------------------------------------------------------------- 1 | classdef RotationParametrizations 2 | enumeration 3 | EULER_ZYZ, TAYT_BRYAN, RPY, TILT_TORSION, QUATERNION 4 | end 5 | end -------------------------------------------------------------------------------- /libs/prototype_log_parser/msgs/ActuatorStatus.m: -------------------------------------------------------------------------------- 1 | classdef ActuatorStatus < handle 2 | properties 3 | id 4 | op_mode 5 | motor_position 6 | motor_speed 7 | motor_torque 8 | cable_length 9 | aux_position 10 | state 11 | pulley_angle 12 | end 13 | 14 | methods 15 | function set(obj, actuator_status_packed) 16 | obj.id = actuator_status_packed(1); 17 | obj.op_mode = actuator_status_packed(2); 18 | obj.motor_position = actuator_status_packed(3); 19 | obj.motor_speed = actuator_status_packed(4); 20 | obj.motor_torque = actuator_status_packed(5); 21 | obj.cable_length = actuator_status_packed(6); 22 | obj.aux_position = actuator_status_packed(7); 23 | obj.state = actuator_status_packed(8); 24 | obj.pulley_angle = actuator_status_packed(9); 25 | end 26 | 27 | function append(obj, actuator_status_packed) 28 | obj.id(end + 1) = actuator_status_packed(1); 29 | obj.op_mode(end + 1) = actuator_status_packed(2); 30 | obj.motor_position(end + 1) = actuator_status_packed(3); 31 | obj.motor_speed(end + 1) = actuator_status_packed(4); 32 | obj.motor_torque(end + 1) = actuator_status_packed(5); 33 | obj.cable_length(end + 1) = actuator_status_packed(6); 34 | obj.aux_position(end + 1) = actuator_status_packed(7); 35 | obj.state(end + 1) = actuator_status_packed(8); 36 | obj.pulley_angle(end + 1) = actuator_status_packed(9); 37 | end 38 | 39 | function clear(obj) 40 | obj.id = []; 41 | obj.op_mode = []; 42 | obj.motor_position = []; 43 | obj.motor_speed = []; 44 | obj.motor_torque = []; 45 | obj.cable_length = []; 46 | obj.aux_position = []; 47 | obj.pulley_angle = []; 48 | obj.state = []; 49 | end 50 | end 51 | end 52 | -------------------------------------------------------------------------------- /libs/prototype_log_parser/msgs/CableRobotMsgs.m: -------------------------------------------------------------------------------- 1 | classdef CableRobotMsgs < uint32 2 | %UNTITLED Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | enumeration 6 | NONE(0) 7 | MOTOR_STATUS(1) 8 | WINCH_STATUS(2) 9 | ACTUATOR_STATUS(3) 10 | end 11 | end 12 | -------------------------------------------------------------------------------- /libs/prototype_log_parser/msgs/MotorStatus.m: -------------------------------------------------------------------------------- 1 | classdef MotorStatus < handle 2 | properties 3 | id 4 | op_mode 5 | motor_position 6 | motor_speed 7 | motor_torque 8 | end 9 | 10 | methods 11 | function set(obj, motor_status_packed) 12 | obj.id = motor_status_packed(1); 13 | obj.op_mode = motor_status_packed(2); 14 | obj.motor_position = motor_status_packed(3); 15 | obj.motor_speed = motor_status_packed(4); 16 | obj.motor_torque = motor_status_packed(5); 17 | end 18 | 19 | function append(obj, motor_status_packed) 20 | obj.id(end + 1) = motor_status_packed(1); 21 | obj.op_mode(end + 1) = motor_status_packed(2); 22 | obj.motor_position(end + 1) = motor_status_packed(3); 23 | obj.motor_speed(end + 1) = motor_status_packed(4); 24 | obj.motor_torque(end + 1) = motor_status_packed(5); 25 | end 26 | 27 | function clear(obj) 28 | obj.id = []; 29 | obj.op_mode = []; 30 | obj.motor_position = []; 31 | obj.motor_speed = []; 32 | obj.motor_torque = []; 33 | end 34 | end 35 | end 36 | -------------------------------------------------------------------------------- /libs/prototype_log_parser/msgs/WinchStatus.m: -------------------------------------------------------------------------------- 1 | classdef WinchStatus < handle 2 | properties 3 | id 4 | op_mode 5 | motor_position 6 | motor_speed 7 | motor_torque 8 | cable_length 9 | aux_position 10 | end 11 | 12 | methods 13 | function set(obj, winch_status_packed) 14 | obj.id = winch_status_packed(1); 15 | obj.op_mode = winch_status_packed(2); 16 | obj.motor_position = winch_status_packed(3); 17 | obj.motor_speed = winch_status_packed(4); 18 | obj.motor_torque = winch_status_packed(5); 19 | obj.cable_length = winch_status_packed(6); 20 | obj.aux_position = winch_status_packed(7); 21 | end 22 | 23 | function append(obj, winch_status_packed) 24 | obj.id(end + 1) = winch_status_packed(1); 25 | obj.op_mode(end + 1) = winch_status_packed(2); 26 | obj.motor_position(end + 1) = winch_status_packed(3); 27 | obj.motor_speed(end + 1) = winch_status_packed(4); 28 | obj.motor_torque(end + 1) = winch_status_packed(5); 29 | obj.cable_length(end + 1) = winch_status_packed(6); 30 | obj.aux_position(end + 1) = winch_status_packed(7); 31 | end 32 | 33 | function clear(obj) 34 | obj.id = []; 35 | obj.op_mode = []; 36 | obj.motor_position = []; 37 | obj.motor_speed = []; 38 | obj.motor_torque = []; 39 | obj.cable_length = []; 40 | obj.aux_position = []; 41 | end 42 | end 43 | end 44 | -------------------------------------------------------------------------------- /libs/prototype_log_parser/parseCableRobotLogFile.m: -------------------------------------------------------------------------------- 1 | function [messages, num_msgs] = parseCableRobotLogFile(filename, show) 2 | %% Safety checks 3 | if ~endsWith(filename, '.log') 4 | error('Error: invalid input file type. Please load a ".log" file.') 5 | end 6 | 7 | fid = fopen(filename, 'r'); 8 | if fid < 0 9 | error('Error: could not open file %s', filename) 10 | end 11 | 12 | %% Init 13 | messages = initMessages(); 14 | counter_empty = 0; 15 | counter_unknown = 0; 16 | 17 | %% Read file 18 | while 1 19 | tline = fgetl(fid); 20 | if ~ischar(tline) 21 | break; 22 | end 23 | split_line = str2num(tline); %#ok 24 | switch split_line(1) 25 | case CableRobotMsgs.NONE 26 | counter_empty = counter_empty + 1; 27 | case CableRobotMsgs.MOTOR_STATUS 28 | messages.motor_status.timestamp(end + 1) = split_line(2); 29 | messages.motor_status.values.append(split_line(3:end)); 30 | case CableRobotMsgs.WINCH_STATUS 31 | messages.winch_status.timestamp(end + 1) = split_line(2); 32 | messages.winch_status.values.append(split_line(3:end)); 33 | case CableRobotMsgs.ACTUATOR_STATUS 34 | messages.actuator_status.timestamp(end + 1) = split_line(2); 35 | messages.actuator_status.values.append(split_line(3:end)); 36 | otherwise 37 | counter_unknown = counter_unknown + 1; 38 | end 39 | end 40 | 41 | fclose(fid); 42 | 43 | %% Count messages 44 | num_msgs.total = 0; 45 | msg_types = fields(messages); 46 | for i = 1:length(msg_types) 47 | message_len = length(messages.(msg_types{i}).timestamp); 48 | if message_len == 0 49 | messages = rmfield(messages, msg_types{i}); 50 | continue 51 | end 52 | num_msgs.(msg_types{i}) = message_len; 53 | num_msgs.total = num_msgs.total + num_msgs.(msg_types{i}); 54 | end 55 | num_msgs.empty = counter_empty; 56 | num_msgs.unknown = counter_unknown; 57 | num_msgs.total = num_msgs.total + counter_empty + counter_unknown; 58 | 59 | %% Plot 60 | if nargin == 1 || ~show 61 | return 62 | end 63 | msg_types = fields(messages); 64 | for i = 1:length(msg_types) 65 | msg_fields = fields(messages.(msg_types{i}).values); 66 | time = messages.(msg_types{i}).timestamp; 67 | figure; 68 | hold on 69 | for j = 1:length(msg_fields) 70 | plot(time, messages.(msg_types{i}).values.(msg_fields{j})) 71 | msg_fields{j} = replace(msg_fields{j}, '_', '\_'); 72 | end 73 | legend(msg_fields) 74 | title(replace(msg_types{i}, '_', '\_')) 75 | xlabel('time [sec]') 76 | xlim([time(1) time(end)]) 77 | hold off 78 | end 79 | 80 | end 81 | 82 | function messages = initMessages() 83 | messages.motor_status = struct('timestamp', [], 'values', MotorStatus); 84 | messages.winch_status = struct('timestamp', [], 'values', WinchStatus); 85 | messages.actuator_status = struct('timestamp', [], ... 86 | 'values', ActuatorStatus); 87 | end 88 | -------------------------------------------------------------------------------- /libs/prototype_log_parser/plot_motor_status.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | %% Parse file 4 | filepath = '/tmp/cable-robot-logs/data.log'; 5 | data = parseCableRobotLogFile(filepath); 6 | 7 | %% Pack according to actuator ID 8 | [sorted_id, sorting_idx] = sort(data.motor_status.values.id); 9 | 10 | actuators_id = unique(sorted_id); 11 | num_actuators = length(actuators_id); 12 | torques_mat = zeros(num_actuators, ceil(length(sorted_id)/num_actuators)); 13 | pulley_enc_mat = torques_mat; 14 | sorted_ts = torques_mat; 15 | for i = 1:num_actuators 16 | idx = sorting_idx(sorted_id == actuators_id(i)); 17 | torques_mat(i, :) = data.motor_status.values.motor_torque(idx); 18 | pulley_enc_mat(i, :) = data.motor_status.values.aux_position(idx); 19 | sorted_ts(i, :) = data.motor_status.timestamp(idx); 20 | end 21 | 22 | %% Plot torques 23 | figure('units','normalized','outerposition',[0 0 1 1]) 24 | for i = 1:num_actuators 25 | subplot(3,1,i) 26 | plot(sorted_ts(i,:), torques_mat(i,:)) 27 | grid on 28 | title(sprintf('Actuator #%d torques', actuators_id(i))) 29 | xlabel('[sec]') 30 | xlim([sorted_ts(i,1), sorted_ts(i,end)]) 31 | end 32 | 33 | %% Plot pulley encoder values 34 | figure('units','normalized','outerposition',[0 0 1 1]) 35 | for i = 1:num_actuators 36 | subplot(3,1,i) 37 | plot(sorted_ts(i,:), pulley_enc_mat(i,:)) 38 | grid on 39 | title(sprintf('Actuator #%d encoder values', actuators_id(i))) 40 | xlabel('[sec]') 41 | xlim([sorted_ts(i,1), sorted_ts(i,end)]) 42 | end -------------------------------------------------------------------------------- /libs/prototype_log_parser/plot_torques.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | %% Parse file 4 | filepath = '/tmp/cable-robot-logs/data.log'; 5 | data = parseCableRobotLogFile(filepath); 6 | 7 | %% Pack according to actuator ID 8 | [sorted_id, sorting_idx] = sort(data.actuator_status.values.id); 9 | 10 | actuators_id = unique(sorted_id); 11 | num_actuators = length(actuators_id); 12 | torques_mat = zeros(num_actuators, length(sorted_id)/num_actuators); 13 | pulley_enc_mat = torques_mat; 14 | pulley_angle_mat = torques_mat; 15 | sorted_ts = torques_mat; 16 | for i = 1:num_actuators 17 | idx = sorting_idx(sorted_id == actuators_id(i)); 18 | torques_mat(i, :) = data.actuator_status.values.motor_torque(idx); 19 | pulley_enc_mat(i, :) = data.actuator_status.values.aux_position(idx); 20 | pulley_angle_mat(i, :) = data.actuator_status.values.pulley_angle(idx); 21 | sorted_ts(i, :) = data.actuator_status.timestamp(idx); 22 | end 23 | 24 | %% Plot torques 25 | figure('units','normalized','outerposition',[0 0 1 1]) 26 | for i = 1:num_actuators 27 | subplot(3,1,i) 28 | plot(sorted_ts(i,:), torques_mat(i,:)) 29 | grid on 30 | title(sprintf('Actuator #%d torques', actuators_id(i))) 31 | xlabel('[sec]') 32 | xlim([sorted_ts(i,1), sorted_ts(i,end)]) 33 | end 34 | %% Plot pulley encoder values 35 | figure() 36 | for i = 1:num_actuators 37 | subplot(3,1,i) 38 | plot(sorted_ts(i,:), pulley_enc_mat(i,:)) 39 | grid on 40 | title(sprintf('Actuator #%d encoder values', actuators_id(i))) 41 | xlabel('[sec]') 42 | xlim([sorted_ts(i,1), sorted_ts(i,end)]) 43 | end 44 | %% Plot pulley angles 45 | figure() 46 | for i = 1:num_actuators 47 | subplot(3,1,i) 48 | plot(sorted_ts(i,:), pulley_angle_mat(i,:)) 49 | grid on 50 | title(sprintf('Actuator #%d pulley angles', actuators_id(i))) 51 | xlabel('[sec]') 52 | xlim([sorted_ts(i,1), sorted_ts(i,end)]) 53 | end -------------------------------------------------------------------------------- /libs/prototype_log_parser/plot_winch_status.m: -------------------------------------------------------------------------------- 1 | clc; clear; close all; 2 | 3 | %% Parse file 4 | filepath = '/tmp/cable-robot-logs/data.log'; 5 | data = parseCableRobotLogFile(filepath); 6 | 7 | %% Pack according to actuator ID 8 | [sorted_id, sorting_idx] = sort(data.winch_status.values.id); 9 | 10 | actuators_id = unique(sorted_id); 11 | num_actuators = length(actuators_id); 12 | MSG_NUM = floor(length(sorted_id)/num_actuators); 13 | torques_mat = zeros(num_actuators, MSG_NUM); 14 | pulley_enc_mat = torques_mat; 15 | sorted_ts = torques_mat; 16 | for i = 1:num_actuators 17 | idx = sorting_idx(sorted_id == actuators_id(i)); 18 | torques_mat(i, :) = data.winch_status.values.motor_torque(idx(1:MSG_NUM)); 19 | pulley_enc_mat(i, :) = data.winch_status.values.aux_position(idx(1:MSG_NUM)); 20 | sorted_ts(i, :) = data.winch_status.timestamp(idx(1:MSG_NUM)); 21 | end 22 | 23 | MAX_STD = 1; 24 | BUFFER_SIZE = 30; 25 | TOUCH_TIME = 134; %SEC 26 | idx = find(sorted_ts(1,:) > TOUCH_TIME); 27 | offset = idx(1); 28 | for j = (BUFFER_SIZE + offset):MSG_NUM 29 | s = std(pulley_enc_mat(:, (j - BUFFER_SIZE):j), 0, 2); 30 | if all( s < MAX_STD) 31 | settling_time = sorted_ts(1, j) 32 | break 33 | end 34 | end 35 | 36 | %% Plot pulley encoder values 37 | figure('units','normalized','outerposition',[0 0 1 1]) 38 | for i = 1:num_actuators 39 | subplot(3,1,i) 40 | plot(sorted_ts(i,:), pulley_enc_mat(i,:)) 41 | hold on 42 | plot([TOUCH_TIME TOUCH_TIME], ylim, '-.') 43 | plot([settling_time settling_time], ylim, '-.') 44 | hold off 45 | grid on 46 | title(sprintf('Actuator #%d encoder values', actuators_id(i))) 47 | xlabel('[sec]') 48 | xlim([sorted_ts(i,1), sorted_ts(i,end)]) 49 | end 50 | 51 | %% Plot torques 52 | figure('units','normalized','outerposition',[0 0 1 1]) 53 | for i = 1:num_actuators 54 | subplot(3,1,i) 55 | plot(sorted_ts(i,:), torques_mat(i,:)) 56 | grid on 57 | title(sprintf('Actuator #%d torques', actuators_id(i))) 58 | xlabel('[sec]') 59 | xlim([sorted_ts(i,1), sorted_ts(i,end)]) 60 | end -------------------------------------------------------------------------------- /libs/under_actuated/AssertStabilityUnderActuated.m: -------------------------------------------------------------------------------- 1 | function [stab_result] = AssertStabilityUnderActuated(cdpr_p,pose) 2 | 3 | cdpr_v = CdprVar(cdpr_p.n_cables); 4 | 5 | cdpr_v = UpdateIKZeroOrd(pose(1:3),pose(4:end),cdpr_p,cdpr_v); 6 | cdpr_v = CalcExternalLoads(cdpr_v,cdpr_p); 7 | 8 | cdpr_v.underactuated_platform = cdpr_v.underactuated_platform.UpdateGeometricJacobians(cdpr_p.underactuated_platform,cdpr_v.geometric_jacobian); 9 | cdpr_v.underactuated_platform = cdpr_v.underactuated_platform.UpdateAnaliticJacobians(cdpr_p.underactuated_platform,cdpr_v.analitic_jacobian); 10 | cdpr_v.platform = cdpr_v.platform.UpdateMassMatrixStateSpace(cdpr_p); 11 | cdpr_v = CalcCablesStaticTension(cdpr_v); 12 | 13 | K_matrix = CalcStiffnessMat(cdpr_v,cdpr_p); 14 | [~,p] = chol(K_matrix); 15 | 16 | if (p>0) 17 | stab_result = 0; 18 | else 19 | stab_result = 1; 20 | end -------------------------------------------------------------------------------- /libs/under_actuated/CalcJacobianGs.m: -------------------------------------------------------------------------------- 1 | function matrix = CalcJacobianGs(cdpr_v) 2 | 3 | n = length(cdpr_v.cable); 4 | K = zeros(6); 5 | 6 | for i=1:n 7 | T = CalcMatrixT(cdpr_v.cable(i)); 8 | a_tilde = Anti(cdpr_v.cable(i).pos_PA_glob); 9 | K = K + cdpr_v.tension_vector(i).*[-T T*a_tilde;-a_tilde*T (a_tilde*T-Anti(cdpr_v.cable(i).vers_t))*a_tilde]; 10 | end 11 | 12 | K(4:6,4:6) = K(4:6,4:6) + Anti(cdpr_v.platform.ext_load(1:3))*Anti(cdpr_v.platform.pos_PG_glob); 13 | matrix = cdpr_v.underactuated_platform.geometric_orthogonal'*K; 14 | 15 | matrix(:,4:end) = matrix(:,4:end)*cdpr_v.platform.H_mat; 16 | 17 | end -------------------------------------------------------------------------------- /libs/under_actuated/CalcJacobianL.m: -------------------------------------------------------------------------------- 1 | function matrix = CalcJacobianL(cdpr_v) 2 | 3 | matrix = cdpr_v.analitic_jacobian; 4 | 5 | end -------------------------------------------------------------------------------- /libs/under_actuated/CalcJacobianStatic.m: -------------------------------------------------------------------------------- 1 | function matrix = CalcJacobianStatic(cdpr_v) 2 | 3 | n = length(cdpr_v.cable); 4 | K = zeros(6); 5 | 6 | for i=1:n 7 | T = CalcMatrixT(cdpr_v.cable(i)); 8 | a_tilde = Anti(cdpr_v.cable(i).pos_PA_glob); 9 | K = K + cdpr_v.tension_vector(i).*[-T T*a_tilde;-a_tilde*T (a_tilde*T-Anti(cdpr_v.cable(i).vers_t))*a_tilde]; 10 | end 11 | 12 | K(4:6,4:6) = K(4:6,4:6) + Anti(cdpr_v.platform.ext_load(1:3))*Anti(cdpr_v.platform.pos_PG_glob); 13 | matrix = K; 14 | 15 | matrix(:,4:end) = matrix(:,4:end)*cdpr_v.platform.H_mat; 16 | 17 | end -------------------------------------------------------------------------------- /libs/under_actuated/CalcJacobianSw.m: -------------------------------------------------------------------------------- 1 | function matrix = CalcJacobianSw(cdpr_v) 2 | 3 | n = length(cdpr_v.cable); m = length(cdpr_v.analitic_jacobian); 4 | matrix = zeros(n,m); 5 | for i=1:n 6 | matrix(i,:) = [cdpr_v.cable(i).vers_w' ... 7 | -cdpr_v.cable(i).vers_w'*Anti(cdpr_v.cable(i).pos_PA_glob)*cdpr_v.platform.H_mat]./... 8 | norm(cdpr_v.cable(i).vers_u'*cdpr_v.cable(i).pos_DA_glob); 9 | end 10 | 11 | end -------------------------------------------------------------------------------- /libs/under_actuated/CalcJacobianTan.m: -------------------------------------------------------------------------------- 1 | function matrix = CalcJacobianTan(cdpr_v) 2 | 3 | n = length(cdpr_v.cable); m = length(cdpr_v.analitic_jacobian); 4 | matrix = zeros(n,m); 5 | for i=1:n 6 | matrix(i,:) = [cdpr_v.cable(i).vers_n' ... 7 | -cdpr_v.cable(i).vers_n'*Anti(cdpr_v.cable(i).pos_PA_glob)*cdpr_v.platform.H_mat]./... 8 | norm(cdpr_v.cable(i).pos_BA_glob); 9 | end 10 | 11 | end -------------------------------------------------------------------------------- /libs/under_actuated/CalcMassMatUnder.m: -------------------------------------------------------------------------------- 1 | function matrix = CalcMassMatUnder(cdpr_v) 2 | 3 | matrix = cdpr_v.underactuated_platform.geometric_orthogonal'*... 4 | cdpr_v.platform.mass_matrix_global*cdpr_v.underactuated_platform.geometric_orthogonal; 5 | 6 | matrix = (matrix+matrix')./2; 7 | 8 | end -------------------------------------------------------------------------------- /libs/under_actuated/CalcMatrixT.m: -------------------------------------------------------------------------------- 1 | function matrix = CalcMatrixT(cable_v) 2 | 3 | matrix = zeros(3); 4 | matrix = matrix + cable_v.vers_w*cable_v.vers_w'.*... 5 | (sin(cable_v.tan_ang)./(cable_v.vers_u'*cable_v.pos_DA_glob)); 6 | matrix = matrix + cable_v.vers_n*cable_v.vers_n'./... 7 | norm(cable_v.pos_BA_glob); 8 | 9 | end -------------------------------------------------------------------------------- /libs/under_actuated/CalcNaturalFrequencies.m: -------------------------------------------------------------------------------- 1 | function [vector,matrix] = CalcNaturalFrequencies(cdpr_p,pose) 2 | 3 | cdpr_v = CdprVar(cdpr_p.n_cables); 4 | 5 | cdpr_v = UpdateIKZeroOrd(pose(1:3),pose(4:end),cdpr_p,cdpr_v); 6 | cdpr_v = CalcExternalLoads(cdpr_v,cdpr_p); 7 | 8 | cdpr_v.underactuated_platform = cdpr_v.underactuated_platform.UpdateGeometricJacobians(cdpr_p.underactuated_platform,cdpr_v.geometric_jacobian); 9 | cdpr_v.platform = cdpr_v.platform.UpdateMassMatrix(cdpr_p); 10 | cdpr_v = CalcCablesStaticTension(cdpr_v); 11 | 12 | K_matrix = CalcStiffnessMatUnder(cdpr_v); 13 | M_matrix = CalcMassMatUnder(cdpr_v); 14 | D_matrix = zeros(length(M_matrix)); 15 | 16 | for i=1:length(D_matrix) 17 | D_matrix(:,i) = linsolve(M_matrix,K_matrix(:,i)); 18 | end 19 | [eigenvectors,eigenvalues_mat] = eig(D_matrix); 20 | 21 | matrix = eigenvectors; 22 | vector = sqrt(diag(eigenvalues_mat))./(2.*pi); 23 | 24 | end -------------------------------------------------------------------------------- /libs/under_actuated/CalcStiffnessMatUnder.m: -------------------------------------------------------------------------------- 1 | function [matrix,matrix_an] = CalcStiffnessMatUnder(cdpr_v) 2 | 3 | n = length(cdpr_v.cable); 4 | K = zeros(6); 5 | for i=1:n 6 | T = CalcMatrixT(cdpr_v.cable(i)); 7 | a_tilde = Anti(cdpr_v.cable(i).pos_PA_glob); 8 | K = K + cdpr_v.tension_vector(i).*[-T T*a_tilde;-a_tilde*T (a_tilde*T-Anti(cdpr_v.cable(i).vers_t))*a_tilde]; 9 | end 10 | D = [eye(3) zeros(3); zeros(3) cdpr_v.platform.H_mat]; 11 | K(4:6,4:6) = K(4:6,4:6) + Anti(cdpr_v.platform.ext_load(1:3))*Anti(cdpr_v.platform.pos_PG_glob); 12 | matrix = -cdpr_v.underactuated_platform.geometric_orthogonal'*K*cdpr_v.underactuated_platform.geometric_orthogonal; 13 | 14 | matrix = (matrix+matrix')./2; 15 | end -------------------------------------------------------------------------------- /libs/under_actuated/CalcUnactuatedAcceleration.m: -------------------------------------------------------------------------------- 1 | function ucdpr_v = CalcUnactuatedAcceleration(ucdpr_v) 2 | 3 | load = [ucdpr_v.total_load_ss_a;ucdpr_v.total_load_ss_u]; 4 | ucdpr_v.unactuated_deriv_2 = ... 5 | linsolve(ucdpr_v.analitic_orthogonal'*ucdpr_v.mass_matrix_global_ss_u,... 6 | ucdpr_v.analitic_orthogonal'*(load-ucdpr_v.mass_matrix_global_ss_a*ucdpr_v.actuated_deriv_2)); 7 | 8 | end -------------------------------------------------------------------------------- /libs/under_actuated/FunDkGsL.m: -------------------------------------------------------------------------------- 1 | function [vector,matrix] = FunDkGsL(cdpr_p,complete_length,variables,varargin) 2 | 3 | cdpr_v = CdprVar(cdpr_p.n_cables); 4 | 5 | [cdpr_v,constraint_l] = CalcKinZeroOrdConstr(variables(1:3),variables(4:end),complete_length,cdpr_p,cdpr_v); 6 | cdpr_v = CalcExternalLoads(cdpr_v,cdpr_p); 7 | cdpr_v.underactuated_platform = cdpr_v.underactuated_platform.UpdateGeometricJacobians(cdpr_p.underactuated_platform,cdpr_v.geometric_jacobian); 8 | [cdpr_v,constraint_gs] = UnderactuatedStaticConstraint(cdpr_v); 9 | 10 | gs_jacobian = CalcJacobianGs(cdpr_v); 11 | l_jacobian = CalcJacobianL(cdpr_v); 12 | vector = [constraint_gs;constraint_l]; 13 | matrix = [gs_jacobian;l_jacobian]; 14 | 15 | if (~isempty(varargin)) 16 | varargin{1}.SetFrame(cdpr_v,cdpr_p); 17 | end 18 | 19 | end -------------------------------------------------------------------------------- /libs/under_actuated/FunDkGsSwL.m: -------------------------------------------------------------------------------- 1 | function [vector,matrix] = FunDkGsSwL(cdpr_p,complete_length,swivel_angles,variables,varargin) 2 | 3 | cdpr_v = CdprVar(cdpr_p.n_cables); 4 | l_constraint = zeros(cdpr_p.n_cables,1); 5 | sw_constraint = l_constraint; 6 | 7 | cdpr_v = UpdateIKZeroOrd(variables(1:3,1),variables(4:end,1),cdpr_p,cdpr_v); 8 | cdpr_v = CalcExternalLoads(cdpr_v,cdpr_p); 9 | cdpr_v = CalcCablesStaticTension(cdpr_v); 10 | 11 | cdpr_v.underactuated_platform = cdpr_v.underactuated_platform.UpdateGeometricJacobians(cdpr_p.underactuated_platform,cdpr_v.geometric_jacobian); 12 | 13 | for i=1:cdpr_p.n_cables 14 | l_constraint(i) = cdpr_v.cable(i).complete_length-complete_length(i); 15 | sw_constraint(i) = cdpr_v.cable(i).swivel_ang-swivel_angles(i); 16 | end 17 | gs_constraint = cdpr_v.underactuated_platform.geometric_orthogonal'*cdpr_v.platform.ext_load; 18 | 19 | l_jacobian = CalcJacobianL(cdpr_v); 20 | sw_jacobian = CalcJacobianSw(cdpr_v); 21 | gs_jacobian = CalcJacobianGs(cdpr_v); 22 | vector = [l_constraint;sw_constraint;gs_constraint]; 23 | matrix = [l_jacobian;sw_jacobian;gs_jacobian]; 24 | 25 | if (~isempty(varargin)) 26 | varargin{1}.SetFrame(cdpr_v,cdpr_p); 27 | end 28 | 29 | end -------------------------------------------------------------------------------- /libs/under_actuated/FunDkStat.m: -------------------------------------------------------------------------------- 1 | function [vector,matrix] = FunDkStat(cdpr_p,cable_tensions,variables,varargin) 2 | 3 | cdpr_v = CdprVar(cdpr_p.n_cables); 4 | cdpr_v = UpdateIKZeroOrd(variables(1:3),variables(4:end),cdpr_p,cdpr_v); 5 | cdpr_v = CalcExternalLoads(cdpr_v,cdpr_p); 6 | cdpr_v.tension_vector = cable_tensions; 7 | vector = -cdpr_v.geometric_jacobian'*cable_tensions+cdpr_v.platform.ext_load; 8 | matrix = CalcJacobianStatic(cdpr_v); 9 | 10 | if (~isempty(varargin)) 11 | varargin{1}.SetFrame(cdpr_v,cdpr_p); 12 | end 13 | 14 | end -------------------------------------------------------------------------------- /libs/under_actuated/FunDkStatL.m: -------------------------------------------------------------------------------- 1 | function [vector,matrix] = FunDkStatL(cdpr_p,complete_length,cable_tensions,variables,varargin) 2 | 3 | cdpr_v = CdprVar(cdpr_p.n_cables); 4 | 5 | [cdpr_v,constraint_l] = CalcKinZeroOrdConstr(variables(1:3),variables(4:end),complete_length,cdpr_p,cdpr_v); 6 | cdpr_v.ext_load = CalcExternalLoads(cdpr_v,cdpr_p); 7 | constraint_static = -cdpr_v.geometric_jacobian'*cable_tensions+cdpr_v.ext_load; 8 | 9 | static_jacobian = CalcJacobianStatic(cdpr_v); 10 | l_jacobian = CalcJacobianL(cdpr_v); 11 | vector = [constraint_static;constraint_l]; 12 | matrix = [static_jacobian;l_jacobian]; 13 | 14 | if (~isempty(varargin)) 15 | varargin{1}.SetFrame(cdpr_v,cdpr_p); 16 | end 17 | 18 | end -------------------------------------------------------------------------------- /libs/under_actuated/FunDkSwL.m: -------------------------------------------------------------------------------- 1 | function [vector,matrix] = FunDkSwL(cdpr_p,complete_length,swivel_angles,variables,varargin) 2 | 3 | cdpr_v = CdprVar(cdpr_p.n_cables); 4 | sw_constraint = zeros(cdpr_p.n_cables,1); 5 | [cdpr_v,l_constraint] = CalcKinZeroOrdConstr(variables(1:3),variables(4:end),complete_length,cdpr_p,cdpr_v); 6 | for i=1:cdpr_p.n_cables 7 | sw_constraint(i) = cdpr_v.cable(i).swivel_angle-swivel_angles(i); 8 | end 9 | 10 | l_jacobian = CalcJacobianL(); 11 | sw_jacobian = CalcJacobianSw(); 12 | vector = [sw_constraint;l_constraint]; 13 | matrix = [sw_jacobian;l_jacobian]; 14 | 15 | if (~isempty(varargin)) 16 | varargin{1}.SetFrame(cdpr_v,cdpr_p); 17 | end 18 | 19 | end -------------------------------------------------------------------------------- /libs/under_actuated/FunGs.m: -------------------------------------------------------------------------------- 1 | function [vector,matrix] = FunGs(cdpr_p,act_vars,un_act_vars,varargin) 2 | 3 | cdpr_v = CdprVar(cdpr_p.n_cables); 4 | 5 | cdpr_v.underactuated_platform = cdpr_v.underactuated_platform.SetVars(0,act_vars,un_act_vars); 6 | pose = cdpr_v.underactuated_platform.RecomposeVars(0,cdpr_p.underactuated_platform); 7 | 8 | cdpr_v = UpdateIKZeroOrd(pose(1:3),pose(4:end),cdpr_p,cdpr_v); 9 | cdpr_v = CalcExternalLoads(cdpr_v,cdpr_p); 10 | 11 | cdpr_v.underactuated_platform = cdpr_v.underactuated_platform.UpdateGeometricJacobians(cdpr_p.underactuated_platform,cdpr_v.geometric_jacobian); 12 | 13 | [cdpr_v,vector] = UnderactuatedStaticConstraint(cdpr_v); 14 | 15 | matrix = CalcJacobianGs(cdpr_v); 16 | matrix(:,cdpr_p.underactuated_platform.actuated_mask) = []; 17 | 18 | if (~isempty(varargin)) 19 | varargin{1}.SetFrame(cdpr_v,cdpr_p); 20 | end 21 | 22 | 23 | 24 | end -------------------------------------------------------------------------------- /libs/under_actuated/GetInfoCdpr.m: -------------------------------------------------------------------------------- 1 | function s = GetInfoCdpr(cdpr_v,t,j,s) 2 | 3 | s.t(1,j) = t; 4 | s.tension_vector(:,j) = cdpr_v.tension_vector; 5 | s.platform.pose(:,j) = cdpr_v.platform.pose; 6 | s.platform.pose_d(:,j) = cdpr_v.platform.pose_d; 7 | s.platform.pose_d_2(:,j) = cdpr_v.platform.pose_d_2; 8 | 9 | for i = 1:length(cdpr_v.cable) 10 | s.cable(i).complete_length(:,j) = cdpr_v.cable(i).complete_length; 11 | s.cable(i).complete_speed(:,j) = cdpr_v.cable(i).complete_speed; 12 | s.cable(i).complete_acceleration(:,j) = cdpr_v.cable(i).complete_acceleration; 13 | s.cable(i).swivel_ang(:,j) = cdpr_v.cable(i).swivel_ang; 14 | s.cable(i).swivel_ang_vel(:,j) = cdpr_v.cable(i).swivel_ang_vel; 15 | s.cable(i).swivel_ang_acc(:,j) = cdpr_v.cable(i).swivel_ang_acc; 16 | s.cable(i).tan_ang(:,j) = cdpr_v.cable(i).tan_ang; 17 | s.cable(i).tan_ang_vel(:,j) = cdpr_v.cable(i).tan_ang_vel; 18 | s.cable(i).tan_ang_acc(:,j) = cdpr_v.cable(i).tan_ang_acc; 19 | end 20 | 21 | 22 | end -------------------------------------------------------------------------------- /libs/under_actuated/IntegrableInverseDynamics.m: -------------------------------------------------------------------------------- 1 | function vect = IntegrableInverseDynamics(cdpr_p,cdpr_v,... 2 | sim_data,index,time,un_act_vars,period,k,geom_fun) 3 | 4 | % Trajectory settings 5 | pStart = sim_data.p(1:cdpr_p.n_cables,index); 6 | pEnd = sim_data.p(1:cdpr_p.n_cables,index+1); 7 | normalizedTime = NormalizedTime(k,time,period); 8 | act_vars = ActuatedDofs(sim_data,normalizedTime,... 9 | geom_fun,pStart,pEnd); 10 | 11 | % Coordinate manipulations 12 | cdpr_v.underactuated_platform = cdpr_v.underactuated_platform.SetVars(0,act_vars(1,:)',... 13 | un_act_vars(1:cdpr_p.pose_dim-cdpr_p.n_cables)); 14 | cdpr_v.underactuated_platform = cdpr_v.underactuated_platform.SetVars(1,act_vars(2,:)',... 15 | un_act_vars(cdpr_p.pose_dim-cdpr_p.n_cables+1:end)); 16 | cdpr_v.underactuated_platform = cdpr_v.underactuated_platform.SetVars(2,act_vars(3,:)',... 17 | zeros(cdpr_p.pose_dim-cdpr_p.n_cables,1)); 18 | pose = cdpr_v.underactuated_platform.RecomposeVars(0,cdpr_p.underactuated_platform); 19 | pose_d = cdpr_v.underactuated_platform.RecomposeVars(1,cdpr_p.underactuated_platform); 20 | 21 | % Model computations 22 | cdpr_v = UpdateIKZeroOrd(pose(1:3),pose(4:end),cdpr_p,cdpr_v); 23 | cdpr_v = UpdateIKFirstOrd(pose_d(1:3),pose_d(4:end),cdpr_p,cdpr_v); 24 | cdpr_v.platform = cdpr_v.platform.UpdateMassMatrixStateSpace(cdpr_p); 25 | cdpr_v = CalcTotalLoadsStateSpace(cdpr_v,cdpr_p); 26 | cdpr_v.underactuated_platform = cdpr_v.underactuated_platform.UpdateDynamicsStateSpace(cdpr_p.underactuated_platform,... 27 | cdpr_v.analitic_jacobian,cdpr_v.platform.mass_matrix_global_ss,cdpr_v.platform.total_load_ss); 28 | cdpr_v = UnderactuatedDynamicsResolution(cdpr_v,cdpr_p.underactuated_platform); 29 | 30 | % Assignment of the state derivative 31 | vect(1:3,1) = cdpr_v.underactuated_platform.unactuated_deriv; 32 | vect(4:6,1) = cdpr_v.underactuated_platform.unactuated_deriv_2; 33 | 34 | end -------------------------------------------------------------------------------- /libs/under_actuated/LookForCloseSolution.m: -------------------------------------------------------------------------------- 1 | function guess = LookForCloseSolution(cdpr_p,ws_par,p) 2 | 3 | guess = []; 4 | for i=1:ws_par.counter 5 | if(norm(p-ws_par.position(:,i))<0.1) 6 | guess = cdpr_p.RotToPar(ws_par.rot_mat(:,:,i)); 7 | break 8 | end 9 | end 10 | end -------------------------------------------------------------------------------- /libs/under_actuated/MassMatrixNormalization.m: -------------------------------------------------------------------------------- 1 | function eigenvectors = MassMatrixNormalization(eigenvectors,matrix) 2 | 3 | for i=1:length(eigenvectors) 4 | scalar = eigenvectors(:,i)'*matrix*eigenvectors(:,i); 5 | eigenvectors(:,i) = eigenvectors(:,i)./sqrt(scalar); 6 | end 7 | 8 | end -------------------------------------------------------------------------------- /libs/under_actuated/UnderActuatedPar.m: -------------------------------------------------------------------------------- 1 | classdef UnderActuatedPar 2 | %PLATFORMPARAMETERS is a class containing static parameters of the platform. 3 | % 4 | % PLATFORMPARAMETERS links the parameter values of the platform from an 5 | % input structure to the homonymous object's properties. 6 | % Parameters consist of inertial properties of the platform and the 7 | % wrench components acting on it. The wrench is evaluated wrt the center 8 | % of mass G. 9 | % 10 | properties 11 | 12 | actuated_mask; 13 | unactuated_mask; 14 | n_cables; 15 | pose_dim; 16 | 17 | end 18 | methods 19 | function obj = UnderActuatedPar(n,n_p) 20 | obj.n_cables = n; 21 | obj.pose_dim = n_p; 22 | obj.actuated_mask = zeros(n,1); 23 | obj.unactuated_mask = zeros(n_p-n,1); 24 | for i=1:n 25 | obj.actuated_mask(i) = i; 26 | end 27 | for i=n+1:n_p 28 | obj.unactuated_mask(i-n) = i; 29 | end 30 | end 31 | 32 | function obj = SetMask(obj,mask) 33 | % CDPRVAR instantiates an object of CableParameters type. 34 | % CDPRVAR defines the object CABLE containing an object for each 35 | % cable that stores time dependent variables of the cable and its 36 | % swivel pulley. 37 | 38 | for i=1:obj.pose_dim 39 | if (mask(i)==1) 40 | obj.actuated_mask = i; 41 | else 42 | obj.unactuated_mask = i; 43 | end 44 | end 45 | end 46 | 47 | end 48 | end -------------------------------------------------------------------------------- /libs/under_actuated/UnderActuatedVar.m: -------------------------------------------------------------------------------- 1 | classdef UnderActuatedVar 2 | %ANDERACTUATEDVAR is a class containing time dependent variables of the regarding under-actuated cdprs. 3 | % 4 | % ANDERACTUATEDVAR updates ... 5 | % 6 | % 7 | properties 8 | 9 | actuated; 10 | unactuated; 11 | 12 | geometric_jacobian_a;% 13 | geometric_jacobian_u;% 14 | geometric_orthogonal; 15 | analitic_jacobian_a;% 16 | analitic_jacobian_u;% 17 | analitic_orthogonal; 18 | 19 | actuated_deriv; 20 | unactuated_deriv; 21 | 22 | actuated_deriv_2; 23 | unactuated_deriv_2; 24 | 25 | mass_matrix_global_a; 26 | mass_matrix_global_u; 27 | 28 | mass_matrix_global_ss_a; 29 | mass_matrix_global_ss_u; 30 | 31 | total_load_a;% 32 | total_load_u;% 33 | 34 | total_load_ss_a;% 35 | total_load_ss_u;% 36 | 37 | external_load_a;% 38 | external_load_u;% 39 | 40 | external_load_ss_a;% 41 | external_load_ss_u;% 42 | 43 | Gamma_mat 44 | 45 | end 46 | methods 47 | 48 | function obj = UpdateGeometricJacobians(obj,par,Jg) 49 | obj.geometric_jacobian_a = Jg(:,par.actuated_mask); 50 | obj.geometric_jacobian_u = Jg(:,par.unactuated_mask); 51 | [n,m] = size(Jg); 52 | 53 | app1 = zeros(n,m-n); 54 | app2 = eye(m-n); 55 | for i = 1:m-n 56 | app1(:,i) = linsolve(-obj.geometric_jacobian_a,obj.geometric_jacobian_u(:,i)); 57 | end 58 | obj.geometric_orthogonal(par.actuated_mask,:) = app1; 59 | obj.geometric_orthogonal(par.unactuated_mask,:) = app2; 60 | end 61 | 62 | function obj = UpdateAnaliticJacobians(obj,par,Ja) 63 | obj.analitic_jacobian_a = Ja(:,par.actuated_mask); 64 | obj.analitic_jacobian_u = Ja(:,par.unactuated_mask); 65 | [n,m] = size(Ja); 66 | 67 | app1 = zeros(n,m-n); 68 | app2 = eye(m-n); 69 | for i = 1:m-n 70 | app1(:,i) = linsolve(-obj.analitic_jacobian_a,obj.analitic_jacobian_u(:,i)); 71 | end 72 | obj.analitic_orthogonal(par.actuated_mask,:) = app1; 73 | obj.analitic_orthogonal(par.unactuated_mask,:) = app2; 74 | end 75 | 76 | function obj = UpdateDynamics(obj,par,J,M,l) 77 | 78 | obj = UpdateGeometricJacobians(obj,par,J); 79 | 80 | obj.mass_matrix_global_a = M(:,par.actuated_mask); 81 | obj.mass_matrix_global_u = M(:,par.unactuated_mask); 82 | 83 | obj.total_load_a = l(par.actuated_mask);% 84 | obj.total_load_u = l(par.unactuated_mask);% 85 | 86 | end 87 | 88 | function obj = UpdateStatics(obj,par,J,l) 89 | 90 | obj = UpdateGeometricJacobians(obj,par,J); 91 | 92 | obj.external_load_a= l(par.actuated_mask);% 93 | obj.external_load_u= l(par.unactuated_mask);% 94 | 95 | end 96 | 97 | function obj = UpdateDynamicsStateSpace(obj,par,J,M,l) 98 | 99 | obj = UpdateAnaliticJacobians(obj,par,J); 100 | 101 | obj.mass_matrix_global_ss_a = M(:,par.actuated_mask); 102 | obj.mass_matrix_global_ss_u = M(:,par.unactuated_mask); 103 | 104 | obj.total_load_ss_a = l(par.actuated_mask);% 105 | obj.total_load_ss_u = l(par.unactuated_mask);% 106 | end 107 | 108 | function obj = UpdateStaticsStateSpace(obj,par,J,l) 109 | 110 | obj = UpdateAnaliticJacobians(obj,par,J); 111 | 112 | obj.external_load_ss_a= l(par.actuated_mask);% 113 | obj.external_load_ss_u= l(par.unactuated_mask);% 114 | 115 | end 116 | 117 | function obj = ExtractVars(obj,par,order,var) 118 | 119 | switch order 120 | case 0 121 | obj.actuated = var(par.actuated_mask); 122 | obj.unactuated = var(par.unactuated_mask); 123 | case 1 124 | obj.actuated_deriv = var(par.actuated_mask); 125 | obj.unactuated_deriv = var(par.unactuated_mask); 126 | case 2 127 | obj.actuated_deriv_2 = var(par.actuated_mask); 128 | obj.unactuated_deriv_2 = var(par.unactuated_mask); 129 | end 130 | 131 | end 132 | 133 | function obj = SetVars(obj,order,var_act,var_un_act) 134 | 135 | switch order 136 | case 0 137 | obj.actuated = var_act; 138 | obj.unactuated = var_un_act; 139 | case 1 140 | obj.actuated_deriv = var_act; 141 | obj.unactuated_deriv = var_un_act; 142 | case 2 143 | obj.actuated_deriv_2 = var_act; 144 | obj.unactuated_deriv_2 = var_un_act; 145 | end 146 | 147 | end 148 | 149 | function var = RecomposeVars(obj,order,par) 150 | 151 | var = zeros(length([obj.actuated;obj.unactuated]),1); 152 | switch order 153 | case 0 154 | var(par.actuated_mask) = obj.actuated; 155 | var(par.unactuated_mask) = obj.unactuated; 156 | case 1 157 | var(par.actuated_mask) = obj.actuated_deriv; 158 | var(par.unactuated_mask) = obj.unactuated_deriv; 159 | case 2 160 | var(par.actuated_mask) = obj.actuated_deriv_2; 161 | var(par.unactuated_mask) = obj.unactuated_deriv_2; 162 | end 163 | 164 | end 165 | 166 | function obj = UpdateReducedTransformationMatrix(obj,mat) 167 | 168 | dims = size(obj.analitic_orthogonal); 169 | obj.Gamma_mat = mat(dims(2)+1:dims(1),:)*obj.analitic_orthogonal; 170 | 171 | end 172 | end 173 | end -------------------------------------------------------------------------------- /libs/under_actuated/UnderactuatedDynamicsResolution.m: -------------------------------------------------------------------------------- 1 | function cdpr_v = UnderactuatedDynamicsResolution(cdpr_v,ucdpr_p) 2 | %UNDERACTUATEDDYNAMICSACCELERATION computes a subset of the acceleration vector of the cdpr. 3 | % 4 | % UNDERACTUATEDDYNAMICSACCELERATION computes the 2nd order time 5 | % derivatives of the unactuated coordinates of an underactuated system. 6 | % The following expression arises from the partition of the system of 7 | % equations of dynamic equilibrium. 8 | % 9 | % CDPR_V is a structure containing time dependent variables of the cdpr. 10 | 11 | cdpr_v.underactuated_platform = CalcUnactuatedAcceleration(cdpr_v.underactuated_platform); 12 | cdpr_v.platform.pose_d_2 = cdpr_v.underactuated_platform.RecomposeVars(2,ucdpr_p); 13 | cdpr_v = CalcCablesDynamicTensionStateSpace(cdpr_v); 14 | 15 | for i=1:length(cdpr_v.cable) 16 | if (isnan(cdpr_v.tension_vector(i))) 17 | cdpr_v.underactuated_platform.unactuated_deriv_2=NaN(length(cdpr_v.underactuated_platform.unactuated_deriv_2),1); 18 | break 19 | end 20 | end 21 | cdpr_v.platform.pose_d_2 = cdpr_v.underactuated_platform.RecomposeVars(2,ucdpr_p); 22 | 23 | end -------------------------------------------------------------------------------- /libs/under_actuated/UnderactuatedStaticConstraint.m: -------------------------------------------------------------------------------- 1 | function [cdpr_v,constraint] = UnderactuatedStaticConstraint(cdpr_v) 2 | 3 | 4 | constraint = cdpr_v.underactuated_platform.geometric_orthogonal'*cdpr_v.platform.ext_load; 5 | cdpr_v = CalcCablesStaticTension(cdpr_v); 6 | 7 | for i=1:length(cdpr_v.cable) 8 | if (isnan(cdpr_v.tension_vector(i))) 9 | constraint=NaN(length(constraint),1); 10 | break 11 | end 12 | end 13 | 14 | end --------------------------------------------------------------------------------