I have been struggling with this a lot and have posted before. However I dove into the toolbox functions and it seems like there is a big mistake. I'm trying to use the simple command rpy2r to make a rotation matrix from roll, pitch, yaw angles. The problem is I give it Radians like the textbook uses but it appears to be expecting degrees and this is when the 'deg' option is not given.
The problem I see is that rpy2r is expecting radians (unless 'deg' is given) it then ensures the angle is in radians. It then sends this to rotz.m which is expecting degrees. Is anyone else having this issue? please help.
-Jason
Watch. Isn't 45 supposed to be radians in this example?
>> rpy2tr([45,0,0])
ans =
1.0000 0 0 0
0 0.7071 -0.7071 0
0 0.7071 0.7071 0
0 0 0 1.0000
however see the code for rpy2r.m
% optionally convert from degrees
if opt.deg
disp('converting from degrees') I added this to see. this convert the given value in degrees to radians. It is not enabled since i didn't use 'deg'.
d2r = pi/180.0;
roll = roll * d2r
pitch = pitch * d2r;
yaw = yaw * d2r;
end
switch opt.order
case {'xyz', 'arm'}
% XYZ order
if numrows(roll) == 1
R = rotx(yaw) * roty(pitch) * rotz(roll);
else
R = zeros(3,3,numrows(roll));
for i=1:numrows(roll)
R(:,:,i) = rotx(yaw(i)) * roty(pitch(i)) * rotz(roll(i));
end
end
case {'zyx', 'vehicle'}
% ZYX order
if numrows(roll) == 1
R = rotz(yaw) * roty(pitch) * rotx(roll); it uses the RPY in radians now with these functions. see below of those functions.
else
R = zeros(3,3,numrows(roll));
for i=1:numrows(roll)
R(:,:,i) = rotz(yaw(i)) * roty(pitch(i)) * rotx(roll(i));
end
end
This is rotz.m
eml_assert_no_varsize(1,gamma);
sigdatatypes.validateAngle(gamma,'rotz','GAMMA',{'scalar'});
% rotate in the direction of x->y, counter-clockwise
rotmat = [cosd(gamma) -sind(gamma) 0; sind(gamma) cosd(gamma) 0; 0 0 1]; Here it is expecting degrees. So this is a problem right?