# Inferring joint orientation on multi-joint arm

Not a robotics question in the strictest sense, I guess, but related closely enough, I hope:

I have an arm-like articulated two(-plus-one)-joint appliance that I want to use as a 3D input device. It uses an angle measurement device and two IMUs, which are placed as depicted schematically below: The blue boxes depict the positioning and orientation of the IMUs (the arrows point to the IMU's relative "forward" direction). The blue dot/highlighted angle represent the angle being measured. The base can rotate around its centre, counting as the third joint, technically, but shouldn't be of too much relevance here. The arm joint rooted at the base has two rotational degrees of freedom, indicated by the red-ish arrows (it can't rotate around the base's up-direction). The unfilled rectangle represents the data I'd like to infer from the other measurements.

edit: I cannot post more accurate schematics here, but if you want to visualise this apparatus a bit better, think of a Geomagic Phantom, except that the lower arm joint is not rotational, but is built more like a classic analog joystick.

Note: The positioning of the sensors, especially of the IMUs at the base and on the second arm joint are fixed, so please do not suggest changing these (I can't).

I'm now wondering how to compute the orientation of the middle link from the data I have: both IMUs return quaternions $q_0$ and $q_2$, respectively (relative to the magnetic north, measured from their relative forward-direction). My representation for the relative rotation between the two arm links is a quaternion ($q_a$) as well (even though it could just as well be directly represented as an angle, but since I'm performing quaternion math anyways, I might as well have it in this form, too). I'm pretty sure that there must be some way to basically compute what an IMU on the middle link (let's call it $q_1$) would measure from the data I have, but I'm not quite sure about my maths here...

My intuition was to compute $q_1 = q_0^{-1} * q_2 * q_a^{-1}$, following from the assumed identity $q_2 = q_0 * q_1 * q_a$, but that doesn't seem to hold. As I feared that the rotation of $q_2$ relative to the joint it resides on influences the computation result, I also computed $q_1 = q_0^{-1} * q_2 * q_{z\pi} * q_a^{-1}$, where $q_{z\pi}$ represents a 90° rotation around the up(=z)-axis.

However, my measurements still seem off when I visualise the movements (the $q_1$ movement seems exaggerated compared to the actually induced movement). What else might I miss here? Is my math faulty, or is it possibly only an implementation mistake I made?

EDIT2: I found that one major flaw of my maths was the lack of calibration. Adding a calibration pose, I was able to compute the relative orientation between the two IMUs in both the visual model and the actual device and go from there. However, to compute the lower link's orientation, I still rely on an equation like $q_1 = q^*_2 * q_a^{-1}$, with $q^*_2$ being the quaternion that rotates from the relative orientation between $q_0$ and $q_2$ in the calibration pose towards their current relative orientation. I'm still not quite sure if that equation is fully appropriate, but it appears to work okay so far.

• Can you provide a better picture? It is hard to visualize what exactly you have. You said it is a 3 link arm, but from the images and text, it looks to me more like a 2 link arm, with 2 DOF at the base, and an elbow. (Ignoring the rotating base).
– Ben
Sep 10, 2017 at 1:01
• I counted the rotating base as an additional link. The arm itself is thus only a two-link system. Sorry for the confusion
– Sty
Sep 11, 2017 at 8:11
• You might find this paper interesting: Low-cost Accelerometers for Robotic Manipulator Perception by Quigley et. al.
– Ben
Aug 2, 2020 at 11:07

What else might I miss here?

1. As pointed out by @Ben, a clear schematics of your robot to make it easily understandable from others.

2. Maybe it's me missing something but IMU are typically accelerometers and gyroscopes. Meaning that the raw outputs are linear accelerations and rotational velocities. Doing some projection of the gravity vector to retrieve orientation is prone to singularities.

3. Your problem seems to me relatively close to some pan-tilt control with 2 IMUs.

4. your intended use is very likely to prove problematic as the nozzel (or end of your arm) will not have the same orientation with respect to the base plan as the altitude evolves

5. Units quaternions represent a unit vector in the base they are expressed in, associated with a rotation around that vector. So you can not use the same 'simple' math than for rotation matrix, meaning that in order to combine rotation you can not simply multiply the quaternion but need a bit more cumbersome operation see.

6. I would suggest you to use transformation matrices composed of rotation matrices and translation vectors. This will prove easy to derivate from your clean model, and useful in the long run when you are going to go toward inverse kinematics of you robots. Note that the change from quaternion to rotation matrix is well documented.

edit : re work the explanation of the quaternion math

• Unfortunately, I don't have clearance to post schematics here, but if it helps visualisation, think of something similar to a Geomagic Phantom geomagic.com/files/9513/4876/1872/Premium_3-small.png (except for the lower joint not rotating, but tilting in two axes). The IMUs we use additionally feature a magnetometer, retrieving an orientation relative to the magnetic north. Pan-tilt control seems like an interesting lead, I'll have a look into that. I don't quite understand your remark regarding quaternion math: (rotation) matrix multiplication is not commutative as well, after all.
– Sty
Sep 29, 2017 at 8:47
• The mechanism was rather clear for me after some thinking but thanks for the picture it cross validates what I guessed. Concerning the math, I was bit fast, I meant that the quaternion product do not naturally lead to quaternions as rotation matrix does, the operation to combine rotation is more complex and detailed in the reference (I will update my original post accordingly). I am not used to magnetometer so I can not elaborate there. Sep 29, 2017 at 9:48

I might be missing something here, but if you are looking for the position of the tip of the arm, it should just be:

$$x = l_1\cos(\beta) + (l_1+l_2)\cos(\alpha + \beta)$$

$$y = l_1\sin(\beta) + (l_1+l_2)\sin(\alpha + \beta)$$

where $$l_1$$ is the length of the first link, $$l_2$$ of the second, $$\alpha$$ the relative angle of rotation that you indicate in your diagram, and $$\beta$$ the angle of rotation of the base arm (not indicated with any symbol in your diagram).