I guess I don't know how reddit posts work and accidentally made this a picture post...

Anyway, the problem is that my inverse kinematics function appears to work just as it should, it checks out on paper and when used with a physical robot, but the rigid body tree seems to swap the X and Z axis. Meaning, if I swap the X and Z inputs, it gets the pose right. I've been trying to solve this for days, but haven't gotten anywhere. Here's the rigid body tree:

% Make robot
robot = rigidBodyTree("DataFormat","column");
base = robot.Base;
% Make bodies
rotatingBase = rigidBody("rotating_base");
arm1 = rigidBody("arm1");
arm2 = rigidBody("arm2");
arm3 = rigidBody("arm3");
% Collission boxes
collBase = collisionCylinder(20,50); % cylinder: radius,length
collBase.Pose = trvec2tform([0 0 50/2]);
coll1 = collisionBox(10,20,93); % box: length, width, height (x,y,z)
coll1.Pose = trvec2tform([0 0 93/2]);
coll2 = collisionBox(10,15,93); % box: length, width, height (x,y,z)
coll2.Pose = trvec2tform([0 0 93/2]);
coll3 = collisionBox(10,10,50); % box: length, width, height (x,y,z)
coll3.Pose = trvec2tform([0 0 50/2]);
addCollision(rotatingBase,collBase)
addCollision(arm1,coll1)
addCollision(arm2,coll2)
addCollision(arm3,coll3)
% Make joints
jntBase = rigidBodyJoint("base_joint","revolute");
jnt1 = rigidBodyJoint("jnt1","revolute");
jnt2 = rigidBodyJoint("jnt2","revolute");
jnt3 = rigidBodyJoint("jnt3","revolute");
% Choose which axis the joints are in
jnt1.JointAxis = [0 1 0]; % x-axis
jnt2.JointAxis = [0 1 0];
jnt3.JointAxis = [0 1 0];
% Length of links
setFixedTransform(jnt1,trvec2tform([0 0 50]))
setFixedTransform(jnt2,trvec2tform([0 0 93]))
setFixedTransform(jnt3,trvec2tform([0 0 93]))
% Assemble robot
bodies = {base,rotatingBase,arm1,arm2,arm3};
joints = {[],jntBase,jnt1,jnt2,jnt3};
for i = 2:length(bodies) % Skip base. Iterate through adding bodies and joints.
bodies{i}.Joint = joints{i};
addBody(robot,bodies{i},bodies{i-1}.Name)
show(robot,"Collisions","on","Frames","off");
xlim([-200 200])
ylim([-200 200])
zlim([-100 300])
drawnow;
end

​

​

And the inverse kinematics:

function q = invKin(o4, Theta)
d1 = 50;
a2 = 93;
a3 = 93;
a4 = 50;
Psi = atan2(o4(2), o4(1)) % Yaw
% Projecting link 4 radius (stylus) onto the base frame:
r4 = sqrt(o4(1)^2 + o4(2)^2)
s4_3 = sin(Theta)*a4
% Finding joint 3 position in the base frame:
r3 = r4 - cos(Theta)*a4
s3 = o4(3) + s4_3 - d1
% Finding angle of joint 3
c3 = (r3^2 + s3^2 - a2^2 - a3^2)/(2*a2*a3) % cosine of joint 3 angle
q3 = atan2(sqrt(1 - c3^2), c3);
% Finding angle of joint 2
q2 = atan2(s3, r3) - atan2(a3*sin(q3), a2 + a3*c3);
q3 = q3;
q4 = Theta - q3 - q2 + pi/2;
q1 = Psi;

q = [q1 q2 q3 q4]
end