SoRoSim Toolbox Update!

Major Updates:
- Toolbox can now handle closed-chain and branched robotic systems in addition to open-chains.
- Scaling techniques are employed for faster computation.
- Major improvements to GUIs and dialogue boxes for an enhanced user experience.
-  Toolbox Class files renamed: the Link class is now SorosimLink, and Linkage is SorosimLinkage. 
- Other general improvements to the code

SoRoSim_V3.0/Basic functions/GaussQuadrature.m

@@ -0,0 +1,68 @@
+% This script is for computing definite integrals using Legendre-Gauss 
+% Quadrature. Computes the Legendre-Gauss nodes and weights  on an interval
+% [a,b] with truncation order N
+%
+% Suppose you have a continuous function f(x) which is defined on [a,b]
+% which you can evaluate at any x in [a,b]. Simply evaluate it at all of
+% the values contained in the x vector to obtain a vector f. Then compute
+% the definite integral using sum(f.*w);
+%
+% Written by Greg von Winckel - 02/25/2004
+% Modified by Anup Teejo Mathew - 20/05/2020
+
+function [Xs,Ws,nGauss]=GaussQuadrature(N_GQ)
+
+a = 0;
+b = 1;
+
+N  = N_GQ-1;
+N1 = N+1; 
+N2 = N+2;
+
+xu=linspace(-1,1,N1)';
+
+% Initial guess
+y = cos((2*(0:N)'+1)*pi/(2*N+2))+(0.27/N1)*sin(pi*xu*N/N2);
+
+% Legendre-Gauss Vandermonde Matrix
+L = zeros(N1,N2);
+
+% Derivative of LGVM
+Lp = zeros(N1,N2);
+
+% Compute the zeros of the N+1 Legendre Polynomial
+% using the recursion relation and the Newton-Raphson method
+
+y0 = 2;
+
+% Iterate until new points are uniformly within epsilon of old points
+while max(abs(y-y0))>eps
+
+
+    L(:,1)  = 1;
+    Lp(:,1) = 0;
+
+    L(:,2)  = y;
+    Lp(:,2) = 1;
+
+    for k=2:N1
+        L(:,k+1) = ( (2*k-1)*y.*L(:,k)-(k-1)*L(:,k-1) )/k;
+    end
+
+    Lp = (N2)*( L(:,N1)-y.*L(:,N2) )./(1-y.^2);   
+
+    y0 = y;
+    y  = y0-L(:,N2)./Lp;
+
+end
+
+% Linear map from[-1,1] to [a,b]
+Xs = (a*(1-y)+b*(1+y))/2;      
+Xs = flip(Xs);
+Xs = [0;Xs;1];
+% Compute the weights
+Ws = (b-a)./((1-y.^2).*Lp.^2)*(N2/N1)^2;
+Ws = [0;Ws;0];
+nGauss = N_GQ+2;
+
+end

SoRoSim_V3.0/Basic functions/dinamico_Adjoint.m

@@ -0,0 +1,8 @@
+function Adj = dinamico_Adjoint(rototras)
+
+Adj          = [rototras(1:3,1:3) zeros(3,3);dinamico_tilde(rototras(1:3,4))*rototras(1:3,1:3) rototras(1:3,1:3)];
+% Adj(1:3,1:3)=rototras(1:3,1:3);
+% Adj(4:6,1:3)=dinamico_tilde(rototras(1:3,4))*rototras(1:3,1:3);
+% Adj(4:6,4:6)=rototras(1:3,1:3);
+
+% eof

SoRoSim_V3.0/Basic functions/dinamico_adj.m

@@ -0,0 +1,8 @@
+function adj = dinamico_adj(screw)
+t1           = dinamico_tilde(screw(1:3));
+adj          = [t1 zeros(3,3);dinamico_tilde(screw(4:6)) t1];
+% adj(1:3,1:3)=dinamico_tilde(screw(1:3));
+% adj(4:6,1:3)=dinamico_tilde(screw(4:6));
+% adj(4:6,4:6)=adj(1:3,1:3);
+
+% eof

SoRoSim_V3.0/Basic functions/dinamico_coAdjoint.m

@@ -0,0 +1,8 @@
+function coAdj = dinamico_coAdjoint(rototras)
+
+coAdj          =[rototras(1:3,1:3) dinamico_tilde(rototras(1:3,4))*rototras(1:3,1:3);zeros(3,3) rototras(1:3,1:3)];
+% coAdj(1:3,1:3)=rototras(1:3,1:3);
+% coAdj(1:3,4:6)=dinamico_tilde(rototras(1:3,4))*rototras(1:3,1:3);
+% coAdj(4:6,4:6)=rototras(1:3,1:3);
+
+% eof

SoRoSim_V3.0/Basic functions/dinamico_coadj.m

@@ -0,0 +1,8 @@
+function coadj = dinamico_coadj(screw)
+t1             = dinamico_tilde(screw(1:3));
+coadj          = [t1 dinamico_tilde(screw(4:6));zeros(3,3) t1];
+% coadj(1:3,1:3)=dinamico_tilde(screw(1:3));
+% coadj(1:3,4:6)=dinamico_tilde(screw(4:6));
+% coadj(4:6,4:6)=coadj(1:3,1:3);
+
+% eof

SoRoSim_V3.0/Basic functions/dinamico_hat.m

@@ -0,0 +1,7 @@
+function se3 = dinamico_hat(screw)
+
+se3          = [dinamico_tilde(screw(1:3)) screw(4:6);zeros(1,4)];
+% se3(1:3,1:3)=dinamico_tilde(screw(1:3));
+% se3(1:3,4)  =screw(4:6);
+
+% eof

SoRoSim_V3.0/Basic functions/dinamico_tilde.m

@@ -0,0 +1,3 @@
+function skew = dinamico_tilde(vec)
+skew          = [0 -vec(3) vec(2);vec(3) 0 -vec(1);-vec(2) vec(1) 0];
+% eof

SoRoSim_V3.0/Basic functions/ginv.m

@@ -0,0 +1,4 @@
+function g = ginv(g)
+g          = [g(1:3,1:3)' -g(1:3,1:3)'*g(1:3,4);0 0 0 1];
+end
+

SoRoSim_V3.0/Basic functions/joint_expmap.m

@@ -0,0 +1,18 @@
+function g = joint_expmap(xi)
+x = 1;
+
+theta      = norm(xi(1:3));
+xihat      = dinamico_hat(xi);
+if theta == 0
+    g      = diag([1 1 1 1])+x*xihat;
+else
+    g      = diag([1 1 1 1])+x*xihat+...
+            ((1-cos(x*theta))/(theta^2))*xihat^2+...
+            ((x*theta-sin(x*theta))/(theta^3))*xihat^3;
+end
+
+
+% eof
+
+
+

SoRoSim_V3.0/Basic functions/piecewise_logmap.m

@@ -0,0 +1,33 @@
+function xci=piecewise_logmap(x,g)
+
+theta = acos(trace(g)/2-1);
+
+if theta==0
+    xcihat      =1/x*(g-diag([1 1 1 1]));
+else
+
+    t0=x*theta;
+    t1=sin(t0);
+    t2=cos(t0);
+    t3=2*t1*t2;
+    t4=1-2*t1^2;
+    t5=t0*t4;
+    gp2=g*g;
+    gp3=gp2*g;
+
+    xcihat      = 1/x*(0.125*(csc(t0/2)^3)*sec(t0/2)...
+                  *((t5-t1)*diag([1 1 1 1])...
+                  -(t0*t2+2*t5-t1-t3)*g...
+                  +(2*t0*t2+t5-t1-t3)*gp2...
+                  -(t0*t2-t1)*gp3));
+
+%               xcihat      =1/x*(0.125*(csc(x*theta/2)^3)*sec(x*theta/2)...
+%         *((x*theta*cos(2*x*theta)-sin(x*theta))*diag([1 1 1 1])...
+%         -(x*theta*cos(x*theta)+2*x*theta*cos(2*x*theta)-sin(x*theta)-sin(2*x*theta))*g...
+%         +(2*x*theta*cos(x*theta)+x*theta*cos(2*x*theta)-sin(x*theta)-sin(2*x*theta))*(g^2)...
+%         -(x*theta*cos(x*theta)-sin(x*theta))*(g^3)));
+%               
+end
+xci = [xcihat(3,2);xcihat(1,3);xcihat(2,1);xcihat(1,4);xcihat(2,4);xcihat(3,4)];
+
+% eof

SoRoSim_V3.0/Basic functions/readme.txt

@@ -0,0 +1,3 @@
+This folder contains several functions that are used for vector/matrix 
+manipulation, mapping from one space to another as well as calculating the
+tangent operator 

SoRoSim_V3.0/Basic functions/variable_Texpmap.m

@@ -0,0 +1,27 @@
+function Texpg = variable_Texpmap(h,theta,Gamma)
+
+adjGamma       = dinamico_adj(Gamma);
+
+if (theta<=1e-7)
+    Texpg      = h*[1 0 0 0 0 0;0 1 0 0 0 0;0 0 1 0 0 0;0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1]+((h^2)/2)*adjGamma;
+else
+
+    tp2 = theta*theta;
+    tp3 = tp2*theta;
+    tp4 = tp3*theta;
+    tp5 = tp4*theta;
+
+    adjGammap2 = adjGamma*adjGamma;
+    adjGammap3 = adjGammap2*adjGamma;
+    adjGammap4 = adjGammap3*adjGamma;
+
+    sin_htheta = sin(h*theta);
+    cos_htheta = cos(h*theta);
+
+    t1 = h*theta*sin_htheta;
+    t2 = h*theta*cos_htheta;
+
+    Texpg      = h*[1 0 0 0 0 0;0 1 0 0 0 0;0 0 1 0 0 0;0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1]+((4-4*cos_htheta-t1)/(2*tp2))*adjGamma+((4*h*theta-5*sin_htheta+t2)/(2*tp3))*adjGammap2+((2-2*cos_htheta-t1)/(2*tp4))*adjGammap3+((2*h*theta-3*sin_htheta+t2)/(2*tp5))*adjGammap4;
+end
+
+% eof

SoRoSim_V3.0/Basic functions/variable_dotTexpmap.m

@@ -0,0 +1,41 @@
+function dotTexpGamma = variable_dotTexpmap(h,theta,dottheta,Gamma,dotGamma)
+
+adj_dotGamma          = dinamico_adj(dotGamma);
+
+if (theta<=1e-7)
+    dotTexpGamma = ((h^2)/2)*adj_dotGamma;
+else
+    adj_Gamma     = dinamico_adj(Gamma);
+    adj_Gammap2   = adj_Gamma*adj_Gamma;
+    adj_Gammap3   = adj_Gammap2*adj_Gamma;
+    adj_Gammap4   = adj_Gammap3*adj_Gamma;
+
+    adj_dot2Gamma = adj_dotGamma*adj_Gamma+adj_Gamma*adj_dotGamma;
+    adj_dot3Gamma = adj_dot2Gamma*adj_Gamma+adj_Gammap2*adj_dotGamma;
+    adj_dot4Gamma = adj_dot3Gamma*adj_Gamma+adj_Gammap3*adj_dotGamma;
+
+    tp2 = theta*theta;
+    tp3 = tp2*theta;
+    tp4 = tp3*theta;
+    tp5 = tp4*theta;
+
+    sin_htheta = sin(h*theta);
+    cos_htheta = cos(h*theta);
+
+    t1 = h*theta*sin_htheta;
+    t2 = h*theta*cos_htheta;
+    t3 = -8+(8-h^2*tp2)*cos_htheta+5*t1;
+    t4 = -8*h*theta+(15-h^2*tp2)*sin_htheta-7*t2;
+
+
+    dotTexpGamma = (dottheta*(t3)/(2*tp3))*adj_Gamma+...
+                   ((4-4*cos_htheta-t1)/(2*tp2))*adj_dotGamma+...
+                   (dottheta*(t4)/(2*tp4))*adj_Gammap2+...
+                   ((4*h*theta-5*sin_htheta+t2)/(2*tp3))*adj_dot2Gamma+...
+                   (dottheta*(t3)/(2*tp5))*adj_Gammap3+...
+                   ((2-2*cos_htheta-t1)/(2*tp4))*adj_dot3Gamma+...
+                   (dottheta*(t4)/(2*theta^6))*adj_Gammap4+...
+                   ((2*h*theta-3*sin_htheta+t2)/(2*tp5))*adj_dot4Gamma;
+end
+
+% eof

SoRoSim_V3.0/Basic functions/variable_expmap.m

@@ -0,0 +1,17 @@
+function g = variable_expmap(theta,Gamma)
+
+Gammahat   = dinamico_hat(Gamma);
+
+if (theta<=1e-7)
+    g      = diag([1 1 1 1])+Gammahat;
+else
+    Gammahatp2 = Gammahat*Gammahat;
+    Gammahatp3 = Gammahatp2*Gammahat;
+    tp2        = theta*theta;
+    tp3        = tp2*theta;
+    g          = diag([1 1 1 1])+Gammahat+...
+                 ((1-cos(theta))/(tp2))*Gammahatp2+...
+                 ((theta-sin(theta))/(tp3))*Gammahatp3;
+end
+
+% eofs