%CINEMATICADIRECTA S-function it is used to compute accelerations and velocities of a % manipulator. % % This S-función calculates the robot acceleration joints.The block entry is % the u vector whose components are positions and velocities of articular % coordinates [q qd] and the robot object called RRed. % % [sys,x0,str,ts] =CINDIR(t,x,u,flag,RRed) % % RRed is an n-axis robot object and describes the manipulator dynamics and % kinematics % % Implementation in Simulink. % % See also: ACCEL_H,ACCEL, ROBOT, ODE45. % 2007 Niceto Luque Sola function [sys,x0,str,ts] =CINDIR(t,x,u,flag,RRed) switch flag, case 0 % initialize robot dimensions [sys,x0,str,ts] = mdlInitializeSizes(RRed); % Init case {3} % come here to calculate derivitives q=[u(1:RRed.n)];% articular postitions vector qd=[u(RRed.n+1:2*RRed.n)];%articular velocities vector tau=[u(2*RRed.n+1:end)]; % external applied torque sys = accel(RRed,q,qd,tau);%inverse dynamic %sys=accel(RRed,u); case {1, 2, 4, 9} sys = []; end % %============================================================================= % mdlInitializeSizes % Return the sizes, initial conditions, and sample times for the S-function. %============================================================================= % function [sys,x0,str,ts]=mdlInitializeSizes(RRed) % % call simsizes for a sizes structure, fill it in and convert it to a % sizes array. % % Note that in this example, the values are hard coded. This is not a % recommended practice as the characteristics of the block are typically % defined by the S-function parameters. % sizes = simsizes; sizes.NumContStates = 0; sizes.NumDiscStates = 0; sizes.NumOutputs = RRed.n; sizes.NumInputs = 3*RRed.n; sizes.DirFeedthrough = 1; sizes.NumSampleTimes = 1; % at least one sample time is needed sys = simsizes(sizes); % % initialize the initial conditions % x0 = []; % % str is always an empty matrix % str = []; % % initialize the array of sample times % ts = [0 0]; % end mdlInitializeSizes