init commit

imstevenpmwork · Aug 30, 2021 · 1da09d6 · 1da09d6
1 parent 2ba4fc1
commit 1da09d6
Show file tree

Hide file tree

Showing 149 changed files with 322,008 additions and 2 deletions.
diff --git a/Humanoid Movement/README.md b/Humanoid Movement/README.md
@@ -0,0 +1,19 @@
+# Humanoid Movement
+
+## Explanation steps for using the simulation
+
+### Setup
+
+* The following instructions were last tested using Windows 10, MATLAB 2020 on December 17th, 2020.
+* Clone the repository of the project in a desired folder from here: https://github.com/imstevenpm/HMHC_Lab2020.git
+* Open MATLAB and be sure to change the working directory to the folder that has the files cloned and the script called main.
+
+### Running the program
+
+* Before running the main script, choose your desired motion by changing the i value on line 29.
+* Choose the desired visualization on line 50 or 51 and comment the other one.
+* Run the main.m script from the MATLAB command line.
+* While the figures appear, press a key when asked in the MATLAB command line.
+* When the two graphs appear, choose the initial and final point of the first graph and then try to choose the same initial and final point in the second graph.
+* The visualization and ground reactions will be plotted.
+* For continue to the next motion, press a key when asked or exit the program by pressing CTRL+C
diff --git a/Humanoid Movement/src/Documents/Data41parameters.ods b/Humanoid Movement/src/Documents/Data41parameters.ods
diff --git a/Humanoid Movement/src/Documents/HMHC_Lab.pdf b/Humanoid Movement/src/Documents/HMHC_Lab.pdf
diff --git a/Humanoid Movement/src/Documents/Manual for ART-Human v1.1.0.pdf b/Humanoid Movement/src/Documents/Manual for ART-Human v1.1.0.pdf
diff --git a/Humanoid Movement/src/Lab_Files/ForceData/custom.csv b/Humanoid Movement/src/Lab_Files/ForceData/custom.csv
diff --git a/Humanoid Movement/src/Lab_Files/ForceData/custom2.csv b/Humanoid Movement/src/Lab_Files/ForceData/custom2.csv
diff --git a/Humanoid Movement/src/Lab_Files/ForceData/customL.csv b/Humanoid Movement/src/Lab_Files/ForceData/customL.csv
diff --git a/Humanoid Movement/src/Lab_Files/ForceData/fastArm.csv b/Humanoid Movement/src/Lab_Files/ForceData/fastArm.csv
diff --git a/Humanoid Movement/src/Lab_Files/ForceData/fastKick.csv b/Humanoid Movement/src/Lab_Files/ForceData/fastKick.csv
diff --git a/Humanoid Movement/src/Lab_Files/ForceData/fastKickArm.csv b/Humanoid Movement/src/Lab_Files/ForceData/fastKickArm.csv
diff --git a/Humanoid Movement/src/Lab_Files/ForceData/jumpFeetUp.csv b/Humanoid Movement/src/Lab_Files/ForceData/jumpFeetUp.csv
diff --git a/Humanoid Movement/src/Lab_Files/ForceData/maxJump.csv b/Humanoid Movement/src/Lab_Files/ForceData/maxJump.csv
diff --git a/Humanoid Movement/src/Lab_Files/ForceData/maxJump2.csv b/Humanoid Movement/src/Lab_Files/ForceData/maxJump2.csv
diff --git a/Humanoid Movement/src/Lab_Files/ForceData/medJump.csv b/Humanoid Movement/src/Lab_Files/ForceData/medJump.csv
diff --git a/Humanoid Movement/src/Lab_Files/ForceData/mediumArm.csv b/Humanoid Movement/src/Lab_Files/ForceData/mediumArm.csv
diff --git a/Humanoid Movement/src/Lab_Files/ForceData/mediumArmNOSTOMP.csv b/Humanoid Movement/src/Lab_Files/ForceData/mediumArmNOSTOMP.csv
diff --git a/Humanoid Movement/src/Lab_Files/ForceData/mediumKick.csv b/Humanoid Movement/src/Lab_Files/ForceData/mediumKick.csv
diff --git a/Humanoid Movement/src/Lab_Files/ForceData/mediumKickArm.csv b/Humanoid Movement/src/Lab_Files/ForceData/mediumKickArm.csv
diff --git a/Humanoid Movement/src/Lab_Files/ForceData/quickJump.csv b/Humanoid Movement/src/Lab_Files/ForceData/quickJump.csv
diff --git a/Humanoid Movement/src/Lab_Files/ForceData/slowArm.csv b/Humanoid Movement/src/Lab_Files/ForceData/slowArm.csv
diff --git a/Humanoid Movement/src/Lab_Files/ForceData/slowKick.csv b/Humanoid Movement/src/Lab_Files/ForceData/slowKick.csv
diff --git a/Humanoid Movement/src/Lab_Files/ForceData/slowKickArm.csv b/Humanoid Movement/src/Lab_Files/ForceData/slowKickArm.csv
diff --git a/Humanoid Movement/src/Lab_Files/MotionData1/custom.drf b/Humanoid Movement/src/Lab_Files/MotionData1/custom.drf
diff --git a/Humanoid Movement/src/Lab_Files/MotionData1/custom2.drf b/Humanoid Movement/src/Lab_Files/MotionData1/custom2.drf
diff --git a/Humanoid Movement/src/Lab_Files/MotionData1/customL.drf b/Humanoid Movement/src/Lab_Files/MotionData1/customL.drf
diff --git a/Humanoid Movement/src/Lab_Files/MotionData1/fastArm.drf b/Humanoid Movement/src/Lab_Files/MotionData1/fastArm.drf
diff --git a/Humanoid Movement/src/Lab_Files/MotionData1/fastKick.drf b/Humanoid Movement/src/Lab_Files/MotionData1/fastKick.drf
diff --git a/Humanoid Movement/src/Lab_Files/MotionData1/fastKickArm.drf b/Humanoid Movement/src/Lab_Files/MotionData1/fastKickArm.drf
diff --git a/Humanoid Movement/src/Lab_Files/MotionData2/jumpFeetUp.drf b/Humanoid Movement/src/Lab_Files/MotionData2/jumpFeetUp.drf
diff --git a/Humanoid Movement/src/Lab_Files/MotionData2/maxJump.drf b/Humanoid Movement/src/Lab_Files/MotionData2/maxJump.drf
diff --git a/Humanoid Movement/src/Lab_Files/MotionData2/maxJump2.drf b/Humanoid Movement/src/Lab_Files/MotionData2/maxJump2.drf
diff --git a/Humanoid Movement/src/Lab_Files/MotionData2/mediumArm.drf b/Humanoid Movement/src/Lab_Files/MotionData2/mediumArm.drf
diff --git a/Humanoid Movement/src/Lab_Files/MotionData2/mediumArmNOSTOMP.drf b/Humanoid Movement/src/Lab_Files/MotionData2/mediumArmNOSTOMP.drf
diff --git a/Humanoid Movement/src/Lab_Files/MotionData3/medJump.drf b/Humanoid Movement/src/Lab_Files/MotionData3/medJump.drf
diff --git a/Humanoid Movement/src/Lab_Files/MotionData3/mediumKick.drf b/Humanoid Movement/src/Lab_Files/MotionData3/mediumKick.drf
diff --git a/Humanoid Movement/src/Lab_Files/MotionData3/mediumKickArm.drf b/Humanoid Movement/src/Lab_Files/MotionData3/mediumKickArm.drf
diff --git a/Humanoid Movement/src/Lab_Files/MotionData3/quickJump.drf b/Humanoid Movement/src/Lab_Files/MotionData3/quickJump.drf
diff --git a/Humanoid Movement/src/Lab_Files/MotionData3/slowArm.drf b/Humanoid Movement/src/Lab_Files/MotionData3/slowArm.drf
diff --git a/Humanoid Movement/src/Lab_Files/MotionData4/slowKick.drf b/Humanoid Movement/src/Lab_Files/MotionData4/slowKick.drf
diff --git a/Humanoid Movement/src/Lab_Files/MotionData4/slowKickArm.drf b/Humanoid Movement/src/Lab_Files/MotionData4/slowKickArm.drf
diff --git a/Humanoid Movement/src/Lab_Files/MotionData4/slowSit.drf b/Humanoid Movement/src/Lab_Files/MotionData4/slowSit.drf
diff --git a/Humanoid Movement/src/MATLAB/BSP_Estimation/BSPparameters.m b/Humanoid Movement/src/MATLAB/BSP_Estimation/BSPparameters.m
@@ -0,0 +1,133 @@
+%% BSPparameters for each segment
+% Gets the mass, intertia matrix and COM of each segment
+% Returns them in the objects
+
+% P should be in same units as the density!
+function [Body]= BSPparameters(P)
+
+  %% Assuming constant density along the whole body
+  p= 1000.0; % kg/m3 (from google)
+
+  %% Hand segment (SE)
+  Hand.a=P(14)/(2*pi);
+  Hand.b=Hand.a;
+  Hand.c=P(2)/2;
+  Hand.p=p; % Density of hand?
+  [Hand.Mass, Hand.COMpos, Hand.Inertia]=semiellipsoid(Hand.p,Hand.a,Hand.b,Hand.c);
+  Hand.COMpos=[-Hand.COMpos, 0,0];
+
+  %% Forearm (ES)
+  Forearm.a0=P(17)/(2*pi);
+  Forearm.b0=Forearm.a0;
+  Forearm.a1=P(15)/(2*pi);
+  Forearm.b1=Forearm.a1;
+  Forearm.L=P(3);
+  Forearm.p=p; % Density of forearm?
+  [Forearm.Mass, Forearm.COMpos, Forearm.Inertia] = ellipticalsolid(Forearm.p,Forearm.L,Forearm.a0,Forearm.b0,Forearm.b0,Forearm.b0,Forearm.a1,Forearm.b1,Forearm.b1,Forearm.b1);
+  Forearm.COMpos=[-Forearm.COMpos, 0,0];
+
+  %% Upperarm (ES)
+  Upperarm.a0=P(18)/(2*pi);
+  Upperarm.b0=Upperarm.a0;
+  Upperarm.a1=P(17)/(2*pi);
+  Upperarm.b1=Upperarm.a1;
+  Upperarm.L=P(5);
+  Upperarm.p=p; % Density of upperarm?
+  [Upperarm.Mass, Upperarm.COMpos, Upperarm.Inertia] = ellipticalsolid(Upperarm.p,Upperarm.L,Upperarm.a0,Upperarm.b0,Upperarm.b0,Upperarm.b0,Upperarm.a1,Upperarm.b1,Upperarm.b1,Upperarm.b1);
+  Upperarm.COMpos=[-Upperarm.COMpos, 0,0];
+
+  %% Foot (ES)
+  Foot.a0=P(19)/(2*pi);
+  Foot.b0=Foot.a0;
+  Foot.a1=(P(33)+P(34))/4;
+  Foot.b1=(P(20)+P(21))/4;
+  Foot.L=P(6);
+  Foot.p=p; % Density of foot?
+  [Foot.Mass, Foot.COMpos, Foot.Inertia] = ellipticalsolid(Foot.p,Foot.L,Foot.a0,Foot.b0,Foot.b0,Foot.b0,Foot.a1,Foot.b1,Foot.b1,Foot.b1);
+  Foot.COMpos=[0 Foot.COMpos 0];
+
+  %% Shank (ES)
+  Shank.a0=P(24)/(2*pi);
+  Shank.b0=Shank.a0;
+  Shank.a1=P(22)/(2*pi);
+  Shank.b1=Shank.a1;
+  Shank.L=P(7);
+  Shank.p=p; % Density of shank?
+  [Shank.Mass, Shank.COMpos, Shank.Inertia] = ellipticalsolid(Shank.p,Shank.L,Shank.a0,Shank.b0,Shank.b0,Shank.b0,Shank.a1,Shank.b1,Shank.b1,Shank.b1);
+  Shank.COMpos=[0,0,Shank.COMpos];
+
+  %% Thigh (ES)
+  Thigh.b0=P(35)/2;
+  Thigh.a0=P(25)/pi-Thigh.b0;
+  Thigh.a1=P(24)/(2*pi);
+  Thigh.b1=Thigh.a1;
+  Thigh.L=P(8);
+  Thigh.p=p; % Density of the thigh?
+  [Thigh.Mass, Thigh.COMpos, Thigh.Inertia] = ellipticalsolid(Thigh.p,Thigh.L,Thigh.a0,Thigh.b0,Thigh.b0,Thigh.b0,Thigh.a1,Thigh.b1,Thigh.b1,Thigh.b1);
+  Thigh.COMpos=[0,0,Thigh.COMpos];
+
+  %% Head (SE)
+  Head.a=P(26)/(2*pi);
+  Head.b=Head.a;
+  Head.c=P(9)/2;
+  Head.p=p; % Density of Head?
+  [Head.Mass, Head.COMpos, Head.Inertia]=semiellipsoid(Head.p,Head.a,Head.b,Head.c);
+  Head.COMpos=[0, 0, -Head.COMpos];
+
+  %% Uppertrunk (ES)
+  Uppertrunk.a0=(P(36)+P(37))/4;
+  Uppertrunk.b0=Uppertrunk.a0;
+  Uppertrunk.a1=Uppertrunk.a0;
+  Uppertrunk.b1=Uppertrunk.a1;
+  Uppertrunk.L=P(7);
+  Uppertrunk.p=p; % Density of the uppertrunk?
+  [Uppertrunk.Mass, Uppertrunk.COMpos, Uppertrunk.Inertia] = ellipticalsolid(Uppertrunk.p,Uppertrunk.L,Uppertrunk.a0,Uppertrunk.b0,Uppertrunk.b0,Uppertrunk.b0,Uppertrunk.a1,Uppertrunk.b1,Uppertrunk.b1,Uppertrunk.b1);
+  Uppertrunk.COMpos=[0, 0, -Uppertrunk.COMpos];
+
+  %% Middletrunk (ES)
+  Middletrunk.a0 = P(37)/2;
+  Middletrunk.a1 = P(38)/2;
+  Middletrunk.L = P(12);
+  Middletrunk.b0 = (P(28)/pi)- Middletrunk.a0;
+  Middletrunk.b1 = (P(29)/pi)- Middletrunk.a1;
+  Middletrunk.p=p; % Density of the middletrunk?
+  [Middletrunk.Mass, Middletrunk.COMpos, Middletrunk.Inertia] = ellipticalsolid(Middletrunk.p,Middletrunk.L,Middletrunk.a0,Middletrunk.b0,Middletrunk.b0,Middletrunk.b0,Middletrunk.a1,Middletrunk.b1,Middletrunk.b1,Middletrunk.b1);
+  Middletrunk.COMpos=[0, 0, -Middletrunk.COMpos];
+
+  %% Lowertrunk (ES)
+  Lowertrunk.a0 = (P(38)+P(39))/4;
+  Lowertrunk.a1 = Lowertrunk.a0;
+  Lowertrunk.L = P(13);
+  Lowertrunk.b0 = ((P(29)+P(30))/(2*pi)) - Lowertrunk.a0; 
+  Lowertrunk.b1 = Lowertrunk.b0; 
+  Lowertrunk.p=p; % Density of the lowertrunk?
+  [Lowertrunk.Mass, Lowertrunk.COMpos, Lowertrunk.Inertia] = ellipticalsolid(Lowertrunk.p,Lowertrunk.L,Lowertrunk.a0,Lowertrunk.b0,Lowertrunk.b0,Lowertrunk.b0,Lowertrunk.a1,Lowertrunk.b1,Lowertrunk.b1,Lowertrunk.b1);  
+  Lowertrunk.COMpos=[0, 0, -Lowertrunk.COMpos];
+
+  %% Wholetrunk
+  Wholetrunk.Mass=Lowertrunk.Mass+Middletrunk.Mass+Uppertrunk.Mass;
+
+  %% Total mass
+  Totalmass=(2*Hand.Mass+2*Forearm.Mass+2*Upperarm.Mass+2*Foot.Mass+2*Shank.Mass+2*Thigh.Mass+Head.Mass+Uppertrunk.Mass+Middletrunk.Mass+Lowertrunk.Mass);
+
+  %% Assembly body structure
+  Body.handL=Hand;
+  Body.forearmL=Forearm;
+  Body.upperarmL=Upperarm;
+  Body.footL=Foot;
+  Body.shankL=Shank;
+  Body.thighL=Thigh;
+  Body.handR=Hand;
+  Body.forearmR=Forearm;
+  Body.upperarmR=Upperarm;
+  Body.footR=Foot;
+  Body.shankR=Shank;
+  Body.thighR=Thigh;
+  Body.head=Head;
+  Body.uppertrunk=Uppertrunk;
+  Body.middletrunk=Middletrunk;
+  Body.lowertrunk=Lowertrunk;
+  Body.wholetrunk=Wholetrunk;
+  Body.totalmass=Totalmass;
+
+end
diff --git a/Humanoid Movement/src/MATLAB/BSP_Estimation/ellipticalsolid.m b/Humanoid Movement/src/MATLAB/BSP_Estimation/ellipticalsolid.m
@@ -0,0 +1,69 @@
+%% Elliptical Solid equations
+% The call should be:
+% ellipticalsolid(p,L,a0,b0,b0,b0,a1,b1,b1,b1)
+
+function [Mass, COMpos, Inertia] = ellipticalsolid(p,L,a0,b0,c0,d0,a1,b1,c1,d1)
+
+  [B1ab, B2ab, B3ab]=B123(a0,b0,c0,d0,a1,b1,c1,d1);
+  [A1ab, A2ab, A3ab]=A123(B1ab,B2ab,B3ab);
+
+  % Assuming a=a,b=b, c=b, d=b (Getting A4abbb)
+  [B4abcd, B5abcd, B6abcd, B7abcd, B8abcd]=B45678(a0,b0,c0,d0,a1,b1,c1,d1);
+  A4abbb= A4(B4abcd,B5abcd,B6abcd,B7abcd,B8abcd);
+
+  % Flipping for a=a, b=a, c=a, d=b (Getting A4aaab)
+  [B4abcd, B5abcd, B6abcd, B7abcd, B8abcd]=B45678(a0,a0,a0,b0,a1,a1,a1,b1);
+  A4aaab= A4(B4abcd,B5abcd,B6abcd,B7abcd,B8abcd);
+
+  Mass= pi*p*L*A1ab;
+  COMpos= L * A2ab/A1ab;
+  Ixx= Mass*A4abbb/(4*A1ab) + Mass*(L^2)*A3ab/A1ab - Mass * (L*A2ab/A1ab)^2;
+  Iyy= Mass*A4aaab/(4*A1ab) + Mass*(L^2)*A3ab/A1ab - Mass* (L*A2ab/A1ab)^2;
+  Izz= Mass*(A4aaab+A4abbb)/(4*A1ab);
+
+  Inertia= diag([Ixx,Iyy,Izz]);
+
+end
+
+
+function [A1ab, A2ab, A3ab] = A123(B1ab,B2ab,B3ab)
+
+  A1ab= B1ab/3 + B2ab/2 + B3ab;
+  A2ab= B1ab/4 + B2ab/3 + B3ab/2;
+  A3ab= B1ab/5  + B2ab/4 + B3ab/3;
+
+end
+
+function [A4abcd]= A4(B4abcd,B5abcd,B6abcd,B7abcd,B8abcd)
+
+  A4abcd= B4abcd/5 + B5abcd/4 + B6abcd/3 + B7abcd/2 + B8abcd;
+
+end
+
+function [B1ab, B2ab, B3ab] = B123(a0,b0,c0,d0,a1,b1,c1,d1)
+
+  B1ab= (a1-a0)*(b1-b0);
+  B2ab= a0*(b1-b0)+b0*(a1-a0);
+  B3ab= a0*b0;
+
+end
+
+function [B4abcd, B5abcd, B6abcd, B7abcd, B8abcd]= B45678(a0,b0,c0,d0,a1,b1,c1,d1)
+  B4abcd= (a1-a0)*(b1-b0)*(c1-c0)*(d1-d0); 
+  B5abcd= a0*(b1-b0)*(c1-c0)*(d1-d0) + ...
+           b0*(a1-a0)*(c1-c0)*(d1-d0)+ ...
+           c0*(a1-a0)*(b1-b0)*(d1-d0)+ ...
+           d0*(a1-a0)*(b1-b0)*(c1-c0);
+  B6abcd= a0*b0*(c1-c0)*(d1-d0)+ ...
+          a0*c0*(b1-b0)*(d1-d0)+ ...
+          a0*d0*(b1-b0)*(c1-c0)+ ...
+          b0*c0*(a1-a0)*(d1-d0)+ ...
+          b0*d0*(a1-a0)*(c1-c0)+ ...
+          c0*d0*(a1-a0)*(b1-b0);
+  B7abcd= b0*c0*d0*(a1-a0)+ ...
+          a0*c0*d0*(b1-b0)+ ...
+          a0*b0*d0*(c1-c0)+ ...
+          a0*b0*c0*(d1-d0);
+  B8abcd= a0*b0*c0*d0;
+
+end
diff --git a/Humanoid Movement/src/MATLAB/BSP_Estimation/plotmeasurements.m b/Humanoid Movement/src/MATLAB/BSP_Estimation/plotmeasurements.m
@@ -0,0 +1,108 @@
+%% Plots the mean of the parameteres measured and their standard deviation
+function [P] = plotmeasurements()
+
+%% Mean computed in the ods file given
+P=[19.4;
+    10;
+    28.36;
+    32.92;
+    34.84;
+    26.98;
+    39.92;
+    39.06;
+    21.96;
+    21.86;
+    25.92;
+    12.76;
+    16.38;
+    28.18;
+    17.56;
+    25.62;
+    26.1;
+    31.63;
+    24.24;
+    23.15;
+    8.34;
+    24.66;
+    34.92;
+    38.9;
+    46.76;
+    55.12;
+    98.18;
+    89.48;
+    79.76;
+    94.22;
+    10.38;
+    7;
+    9.42;
+    2.82;
+    18.34;
+    32.62;
+    29.96;
+    31.22;
+    30.7;
+    30.12;
+    57.725];
+
+% Standard deviation computed from the ods file given
+std=[
+    1.019803903;
+    0.8093207028;
+    2.532390175;
+    1.325518766;
+    2.107842499;
+    0.3898717738;
+    6.106308214;
+    5.784289066;
+    1.504327092;
+    4.366119559;
+    5.738640954;
+    9.92537153;
+    2.526262061;
+    0.8348652586;
+    0.8414273587;
+    1.577022511;
+    0.6284902545;
+    0.882892972;
+    1.543696861;
+    3.228002478;
+    0.4449719092;
+    2.283199509;
+    3.645819524;
+    4.068169121;
+    7.375838935;
+    9.069013177;
+    1.407835218;
+    5.123670559;
+    2.746452257;
+    1.302689526;
+    0.8786353055;
+    1.027131929;
+    1.249799984;
+    0.4324349662;
+    3.041874422;
+    1.948589233;
+    2.170944495;
+    2.587856256;
+    2.196588264;
+    2.982783935;
+    5.952800461;
+    ];
+
+%% Plots the values
+figure;
+e = errorbar(P,std,'o');
+str='#f9a800';
+color = sscanf(str(2:end),'%2x%2x%2x',[1 3])/255;
+title('Hanavan Parameters Mean and Standard Deviation', 'FontSize',12,'FontWeight','bold','Color',color);
+xlabel('Parameter Number');
+ylabel('cm');
+
+e.Color = 'black';
+e.CapSize = 10;
+
+%fname = 'C:\Users\Steven\Desktop\EMARO 2ND YEAR\HMHC\lab\repo2final\src\Report\Figures';
+%filename=strcat('Hanavan Parameters');
+%saveas(gca,fullfile(fname, filename), 'jpeg');
+
+end
diff --git a/Humanoid Movement/src/MATLAB/BSP_Estimation/semiellipsoid.m b/Humanoid Movement/src/MATLAB/BSP_Estimation/semiellipsoid.m
@@ -0,0 +1,12 @@
+%% Semi-Ellipsoid equations
+function [Mass, COMpos, Inertia] = semiellipsoid(p,a,b,c)
+
+  Mass= 2*pi*p*a*b*c/3;
+  COMpos= 3*c/8;
+  Ixx= ((b^2+c^2)-(3*c/8)^2)*Mass/5;
+  Iyy= ((a^2+c^2)-(3*c/8)^2)*Mass/5;
+  Izz= (a^2+b^2)*Mass/5;
+
+  Inertia=diag([Ixx,Iyy,Izz]);
+
+end
diff --git a/Humanoid Movement/src/MATLAB/Energies/central_difference.m b/Humanoid Movement/src/MATLAB/Energies/central_difference.m
@@ -0,0 +1,40 @@
+%% Computes the central derivate of the vector
+% We get the velocity and acceleration on x,y and z of each joint for each
+% timestep
+function [v, vd, w, wd] = central_difference(pose,ori,time)
+
+    %velocities, differences between positions and orientations over time
+    for i=2:length(pose)-1
+        v(:,:,i-1) = (pose(:,:,i+1)-pose(:,:,i-1))/(time(i+1)-time(i-1));
+        w(:,:,i-1)= (ori(:,:,i+1)-ori(:,:,i-1))/(time(i+1)-time(i-1));
+    end
+
+    % Apply median filter to remove spikes, then low pass filter with fc=4
+    [b,a]=butter(4,4/((1/(2*(time(i+1)-time(i-1))))/2));
+    for i=1:size(v,1)
+        for j=1:size(v,2)
+            v(i,j,:) = medfilt1(squeeze(v(i,j,:)),11);
+            w(i,j,:) = medfilt1(squeeze(w(i,j,:)),11);
+            v(i,j,:)=filtfilt(b,a,squeeze(v(i,j,:)));
+            w(i,j,:)=filtfilt(b,a,squeeze(w(i,j,:)));
+
+        end
+    end
+
+    %accelerations, differences between velocities over time
+    for i=3:length(pose)-3
+        vd(:,:,i-2) = (v(:,:,i+1)-v(:,:,i-1))/(time(i+1)-time(i-1));
+        wd(:,:,i-2)= (w(:,:,i+1)-w(:,:,i-1))/(time(i+1)-time(i-1));
+    end
+
+    % Apply median filter to remove spikes, then low pass filter with fc=4
+    for i=1:size(v,1)
+        for j=1:size(v,2)
+            vd(i,j,:) = medfilt1(squeeze(vd(i,j,:)),11);
+            wd(i,j,:) = medfilt1(squeeze(wd(i,j,:)),11);
+            vd(i,j,:)=filtfilt(b,a,squeeze(vd(i,j,:)));
+            wd(i,j,:)=filtfilt(b,a,squeeze(wd(i,j,:)));
+        end
+    end
+
+end