# 4 axis orientation only robot solving inverse kinematics

I'm working on a 4 axis robot that only changes orientation (every axis is revolute and rotates around a common point). My goal is to be able to provide a look vector (x,y,z) and have the robots tool point in that direction (converting 4 axes into 3 axes). I'm relatively new to robotics and am learning about Jacobian IK but I want to make sure that I'm going down a good path or if anyone has suggestions for other methods of solving this robot.

Here's an illustration of the robot:  In the robots home positive, roll is redundant to train. The pan axis has limited rotation of +/- 15° so that the tool doesn't hit the tilt arms. Tilt also is constrained +/- 240° so that the tool doesn't hit the train arms. Train and Roll have slip rings so they can infinitely spin.

With this, if I wanted to have the robot look directly left (1,0,0) I would spin the train axis 90°, tilt 90° and roll -90°. That's the easy use case, but becomes more challenging if I wanted to have it look at vector (1,1,1). In this case there's a lot of valid combinations of rotations to make it look in that direction and solving it with trig doesn't seem feasible and needs to be an iterative solution.

I've been watching lots of videos on DH tables, homogeneous transforms and now Jacobians but I don't have a big math background so its a lot to take in. Also most examples are for arm like robots and not just orientation. I want to make sure that before I spend a lot more time looking into Jacobian methods that its a good path forward. There also seems to be a lot of options for Jacobian methods such as transpose, pseudoinverse, dampened least squares and I'm not sure which one to look more into.

Alternatively, I'm not tied to a Jacobian method if anyone has any other suggestions on algorithms that would work well for this. I've used FABRIK before but that doesn't seem appropriate here as it seems more suited for solving chains.

Based on your comment I rewrote the answer. I was under the impression that you want to control a 3D orientation; however, what you seem to aim for is a pointing/ray style target. This is actually a 2D target (the position on a 3D unit sphere) and means you have one more DoF in your null space. The solution, however, is quite similar in either case.

I also added a more detailed derivation to - hopefully - make it clear where things come from.

Hope this helps.

We can express the orientation of the tool frame in the world frame by tracing all the rotations used in the kinematic chain:

$$^\mathrm{world}\mathbf{R}_\mathrm{tool} = \mathbf{R}_z(\theta_\textrm{roll})\mathbf{R}_x(-90°)\mathbf{R}_z(90°)\\ \mathbf{R}_z(\theta_\textrm{pan})\mathbf{R}_x(90°)\mathbf{R}_z(-90°) \mathbf{R}_z(\theta_\textrm{tilt})\mathbf{R}_x(90°) \mathbf{R}_z(\theta_\textrm{train})$$

In the tool frame, we know the direction of the ray, which is a constant $$\mathbf{v}=(0, 0, 1)=\mathbf{e}_\mathrm{z}$$ and we can work out the direction of the ray in world coordinates by taking the inverse of the above chain of rotations. This gives us the general forward kinematics:

$$FK(\theta_\textrm{train}, \theta_\textrm{tilt}, \theta_\textrm{pan}, \theta_\textrm{roll})= {^\mathrm{world}\mathbf{R}}_\mathrm{tool}^{-1}(\theta_\textrm{train}, \theta_\textrm{tilt}, \theta_\textrm{pan}, \theta_\textrm{roll})~\mathbf{v}$$

Getting that inverse rotation is simple, because - as you probably know - $$\mathbf{R}^{-1}=\mathbf{R}^T$$, $$\mathbf{R}^{-1}(\alpha)=\mathbf{R}(-\alpha)$$, and $$(\mathbf{A}\mathbf{B})^T=\mathbf{B}^T\mathbf{A}^T$$.

This gives us a (lengthy) formula for the FK:

$$FK(\theta) = \mathbf{R}_z(-\theta_\textrm{train}) \mathbf{R}_x(-90°)\mathbf{R}_z(-\theta_\textrm{tilt})\\ \mathbf{R}_z(90°)\mathbf{R}_x(-90°)\mathbf{R}_z(-\theta_\textrm{pan}) \mathbf{R}_z(-90°)\mathbf{R}_x(90°)\mathbf{R}_z(-\theta_\textrm{roll})\mathbf{e}_\mathrm{z}$$

At this point, we could use CCD (cyclic coordinate descent) to solve the IK problem and find the joint angles by choosing the current configuration as the initial condition.

We can compute the Jacobian using the product rule $$\mathbb{d}(\mathbf{A}\mathbf{x})/\mathbb{d}\theta_i = \delta\mathbf{A}/\delta\theta_i~\mathbf{x} +\mathbf{A}~\mathbb{d}\mathbf{x}/\mathbb{d}\theta_i$$ and the derivative of a rotation matrix $$\delta\mathbf{R_z(\alpha)}/\delta\alpha = \mathbf{S}((0,0,1))\mathbf{R}_z(\alpha)$$, where $$\mathbf{S}$$ is the skew-symmeric matrix in 3D that associates with the cross product.

Since any $$\theta_i$$ only occurs once in the chain, the derivative is straightforward. We simply replace the respective $$\mathbf{R}_z(-\theta_i)$$ in the $$FK$$ equation with $$\mathbf{S}(-\mathbf{e}_z)\mathbf{R}_z(-\theta_i)$$ and obtain the respective column of the Jacobian.

Here is one example column:

$$\mathbf{J}_{pan}=\mathbf{R}_z(-\theta_\textrm{train}) \mathbf{R}_x(-90°)\mathbf{R}_z(-\theta_\textrm{tilt})\\ \mathbf{R}_z(90°)\mathbf{R}_x(-90°)\mathbf{S}(-\mathbf{e}_z)\mathbf{R}_z(-\theta_\textrm{pan}) \mathbf{R}_z(-90°)\mathbf{R}_x(90°)\mathbf{R}_z(-\theta_\textrm{roll})\mathbf{v}$$

Notice that $$\mathbf{J}_\mathrm{roll} = 0$$ because $$\mathbf{S}(-\mathbf{e}_z)\mathbf{R}_z(-\theta_\textrm{roll})\mathbf{v} = \mathbf{S}(-\mathbf{e}_z)\mathbf{v} = 0$$.

With this, we get access to all the variants of Jacobian-based IK, e.g., inverse jacobian.

Solving the IK problem analytically would mean that we wish to find all combinations of $$(\theta_\textrm{train}, \theta_\textrm{tilt}, \theta_\textrm{pan}, \theta_\textrm{roll})$$ such that $$\mathbf{v}_\mathrm{target} = FK(\theta_\textrm{train}, \theta_\textrm{tilt}, \theta_\textrm{pan}, \theta_\textrm{roll})$$.

We have 4 unknowns, so we will need 4 (non-redundant) equations. You might think that we can get 3 from the above $$\mathbf{v}_\mathrm{target} = FK(\theta)$$, but we can actually only get 2. Although we have formulated the problem in 3D, we are moving around on the unit sphere, which is a 2D subspace. This becomes particularly clear if we transform the entire problem into spherical coordinates and then look at the radius $$r$$ component:

$$r_\textrm{FK} = |FK(\theta)| = 1 = |\mathbf{v}_\mathrm{target}| = r_\textrm{target}$$

The above is independent of $$\theta$$ and hence a redundant equation. Hence we will have to "add" two equations/constraints while solving the IK.

The first equation shall be $$\theta_\textrm{roll} = 0$$, because $$\mathbf{R}_z(-\theta_\textrm{roll})\mathbf{v} = \mathbf{v}$$. We can also see this from the Jacobian of $$\theta_\textrm{roll}$$, which is always be 0. Hence, a solution of the IK will be a solution regardless of the angle we choose here. This reduces the number of unknowns down to 3:

$$\mathbf{v}_\mathrm{target} = \mathbf{R}_z(-\theta_\textrm{train}) \mathbf{R}_x(-90°)\mathbf{R}_z(-\theta_\textrm{tilt}) \mathbf{R}_z(90°)\mathbf{R}_x(-90°)\mathbf{R}_z(-\theta_\textrm{pan}) (-\mathbf{e}_x)$$

We can then do some reordering (variable terms to the front, constant terms towards the back) via $$\mathbf{R}_x(-90°)\mathbf{R}_z(\alpha) = \mathbf{R}_y(-\alpha)\mathbf{R}_x(-90°)$$:

$$\mathbf{v}_\mathrm{target} = \mathbf{R}_z(-\theta_\textrm{train}) \mathbf{R}_y(\theta_\textrm{tilt})\mathbf{R}_x(-90°) \mathbf{R}_z(90°)\mathbf{R}_y(\theta_\textrm{pan})\mathbf{R}_x(-90°) (-\mathbf{e}_x)$$

and $$\mathbf{R}_z(90°)\mathbf{R}_y(\alpha) = \mathbf{R}_x(\alpha)\mathbf{R}_z(90°)$$:

$$\mathbf{v}_\mathrm{target} = \mathbf{R}_z(-\theta_\textrm{train}) \mathbf{R}_y(\theta_\textrm{tilt})\mathbf{R}_x(-90°) \mathbf{R}_x(\theta_\textrm{pan})\mathbf{R}_z(90°)\mathbf{R}_x(-90°) (-\mathbf{e}_x)$$

and finally $$\mathbf{R}_x(-90°)\mathbf{R}_x(\alpha) = \mathbf{R}_x(\alpha)\mathbf{R}_x(-90°)$$:

$$\mathbf{v}_\mathrm{target} = \mathbf{R}_z(-\theta_\textrm{train}) \mathbf{R}_y(\theta_\textrm{tilt}) \mathbf{R}_x(\theta_\textrm{pan})\mathbf{R}_x(-90°)\mathbf{R}_z(90°)\mathbf{R}_x(-90°) (-\mathbf{e}_x)$$

We can then combine all the constant terms:

$$\mathbf{v}_\mathrm{target} = \mathbf{R}_z(-\theta_\textrm{train}) \mathbf{R}_y(\theta_\textrm{tilt}) \mathbf{R}_x(\theta_\textrm{pan})\mathbf{e}_z$$

Unsurprisingly so, we can identify the above as the unit vector along the tool's z-axis being rotated by a xyz intrinsic Euler rotation (from the tool frame into the world frame).

At this point we can make a choice for the second equation we need to add and pick $$\theta_\mathrm{pan}=0$$. This would reduce the problem to

$$\mathbf{v}_\mathrm{target} = \mathbf{R}_z(-\theta_\textrm{train}) \mathbf{R}_x(\theta_\textrm{tilt})\mathbf{e}_z$$

which is exactly the two angles measured by your standardspherical coordinate system (mind a minus):

$$\theta_\mathrm{train} = -\arccos(v_{\mathrm{target},3})$$ $$\theta_\mathrm{tilt} =\operatorname{atan2}(v_{\mathrm{target},2}/v_{\mathrm{target},1})$$

This gives us one of the various IK solutions:

$$\theta_\mathrm{train} = -\arccos(v_{\mathrm{target},3})\\ \theta_\mathrm{tilt} =\operatorname{atan2}(v_{\mathrm{target},2}/v_{\mathrm{target},1})\\ \theta_\mathrm{pan}=0\\ \theta_\mathrm{roll}=0\\$$

The above works if you just want to steer to a given position. If you want to do some kind of tracking or use cartesian control, however, this will fail once $$\theta_\mathrm{tilt}$$ approaches $$\pm 180°$$. There, the robot/gimbal will do a flip/spin to the other side and lose track or deviate significantly from the desired trajectory.

You can avoid that by involving $$\theta_\mathrm{pan}$$. Another, easier, way would be to make your tool face along any of roll`'s axes that isn't the z-axis. That way you get two joints without constraints and will never run into this problem.

• I can ignore roll as the spin around the look direction can be calculated elsewhere. Changing it to give zxy angles, a look angle of (0,1,0) that gives me a a tilt of -90 degrees. That makes sense as it would point directly up. Should I change this to desired (1,0,0) aka looking left, this gives me only pan 90. This is where I'm not sure I understand how this should work. My pan axis only has a limited rotation of +/-15 degrees so pan cannot be the only axis used to look side to side. Instead, a combination of train/tilt would be needed. Id really expect something like (90,90,0) Oct 8, 2021 at 20:50
• @JoeJankowiak I updated/rewrote the answer. Hopefully, this will make things more clear :) Oct 9, 2021 at 18:48
• Thank you for such a detailed explanation!! I also realized i made a mistake in limits for tilt, which should be ±140 so I won't run into the flip scenario you described. It'll take me a bit to fully digest this and code it but this looks very promising so I'll accept it as the answer. Oct 10, 2021 at 4:57

Since the 3 axes intersect at a single point I'd recommend thinking about having a gimbal or spherical wrist instead of having a 4 dof robot. This way you don't need to bother with DH params.

With this view, your forward and inverse kinematics (for the first 3 dofs) will involve manipulating the resulting roll-pitch-yaw Euler angle rotation matrix.

I would do a search for "robot spherical wrist inverse kinematics" and go through some of the lecture notes and videos that come up and go through this step-by-step. I think this will simplify your kinematics.

You didn't mention anything about your 4th dof so I'm assuming once you're pointed in the right direction you already know how to find the angle for the 4th dof. Or, if you want to use this redundant dof, I'd refer you to scholar.google.com for papers on control of 4 dof gimbals.

• 4 axes could be to avoid gimbal lock? although it is not clear to me whether this does avoid gimbal lock Oct 8, 2021 at 15:54
• I agree, a spherical wrist probably would have been better given our payload tool is pretty light. We are repurposing a 4 axis camera head we make (but didn't program) for a prototype project. The 4th axis when used as a camera head is for gimbal lock, but we have a smaller working envelope and I don't think we would encounter gimbal lock (but its there if needed). Thanks for the resource for papers, I'll check it out Oct 8, 2021 at 16:50