SE3Rodrigues.m

function T = SE3Rodrigues(in1,theta,in3)
%SE3Rodrigues
%    T = SE3Rodrigues(Omega,theta,v)

%    This function was generated by the Symbolic Math Toolbox version 9.1.
%    24-Jun-2022 16:43:30

Omg1 = in1(1,:);
Omg2 = in1(2,:);
Omg3 = in1(3,:);
v1 = in3(1,:);
v2 = in3(2,:);
v3 = in3(3,:);
t2 = cos(theta);
t3 = sin(theta);
t4 = Omg1.*v2;
t5 = Omg2.*v1;
t6 = Omg1.*v3;
t7 = Omg3.*v1;
t8 = Omg2.*v3;
t9 = Omg3.*v2;
t10 = Omg1.^2;
t11 = Omg2.^2;
t12 = Omg3.^2;
t13 = Omg1.*t3;
t14 = Omg2.*t3;
t15 = Omg3.*t3;
t16 = -t5;
t17 = -t7;
t18 = -t9;
t19 = t2-1.0;
t22 = Omg1.*Omg2.*t19;
t23 = Omg1.*Omg3.*t19;
t24 = Omg2.*Omg3.*t19;
t25 = t10.*t19;
t26 = t11.*t19;
t27 = t12.*t19;
t28 = t4+t16;
t29 = t6+t17;
t30 = t8+t18;
t31 = -t22;
t32 = -t23;
t33 = -t24;
T = reshape([t26+t27+1.0,t15+t31,-t14+t32,0.0,-t15+t31,t25+t27+1.0,t13+t33,0.0,t14+t32,-t13+t33,t25+t26+1.0,0.0,-t19.*t30+theta.*v1-(Omg2.*t28+Omg3.*t29).*(t3-theta),t19.*t29+theta.*v2+(Omg1.*t28-Omg3.*t30).*(t3-theta),-t19.*t28+theta.*v3+(Omg1.*t29+Omg2.*t30).*(t3-theta),1.0],[4,4]);