18 lines
414 B
Mathematica
18 lines
414 B
Mathematica
|
function R = RPY(q)
|
||
|
|
%RPY
|
||
|
|
% R = RPY(IN1)
|
||
|
|
|
||
|
|
% This function was generated by the Symbolic Math Toolbox version 8.0.
|
||
|
|
% 19-Mar-2018 11:39:28
|
||
|
|
|
||
|
|
q1 = q(:,1);
|
||
|
|
q2 = q(:,2);
|
||
|
|
q3 = q(:,3);
|
||
|
|
t2 = cos(q3);
|
||
|
|
t3 = sin(q1);
|
||
|
|
t4 = sin(q3);
|
||
|
|
t5 = cos(q1);
|
||
|
|
t6 = sin(q2);
|
||
|
|
t7 = cos(q2);
|
||
|
|
R = reshape([t2.*t7,t4.*t7,-t6,-t4.*t5+t2.*t3.*t6,t2.*t5+t3.*t4.*t6,t3.*t7,t3.*t4+t2.*t5.*t6,-t2.*t3+t4.*t5.*t6,t5.*t7],[3,3]);
|