# Humanoid robot arm - inverse kinematics - choose joint solutions

I have developed a 7DOF arm for a humanoid robot (see pic below for more details) I have implemented the IK using a closed form solution and of course I come up with eight solutions - each one is actually positioning the end effector at the right position and orientation (I implemented the method described in the paper: "Kinematics and Inverse Kinematics for the Humanoid Robot HUBO2+"). The question now is how to choose the right one, knowing the fact that the end effector will follow a trajectory. The idea is to compute iteratively the $[N, S, A, P]$ matrix that will be provided to the IK module.

One solution I am thinking off is to choose the joint solution in the decision table that will minimize the given metrics: $$\sum_i (\theta_i^{current} - \theta_i^{next})^2$$ where $\theta_i^{current}$ represents the the current value of the $i^{th}$ joint and $\theta_i^{next}$ is the computed value in the decision table.

Do you think it is the right approach or there are other methods out there to find the best joint solution from the decision table.

That looks like a fun pair of arms :-)

Yes, if you already have a known pose, choosing a solution that minimizes movement is a decent choice. The obvious draw-back is that which choice is made for a particular pose then depends on history, which may make validation harder.

Another choice would be the one that minimizes the torque (because of gravity/load) on the weaker servos. I notice that you use a MX-28 in the shoulder, but AX-12 in the elbow. With the weight of the gripper and load, and the length of your arms, you should probably not plan on lifting any great weights in that configuration!

I was asked to elaborate on "obvious draw-back."

When validating that a mechanical system is safe to use in a particular environment, such as around human beings, you need to be able to validate that the system will never be in a "bad" configuration. When the configuration of time T depends on the configuration in time T-1, then you can't create a closed-form formula that describes exactly where the system will be for a given set of inputs, and thus validation degrades to either assuming the most pessimistic assumptions (everything within the theoretically possible movement range will happen,) or you have to degrade to manual testing, which is notoriously bad at finding rare edge cases.

Similarly, when something goes wrong, you're going to have a hard time reproducing the conditions for the failure, so that you can study them, fix them, and then add some kind of test to make sure the failure will never happen again. If the pose of an armature is dependent not just on known current inputs, but on all inputs that have been fed to the robot since power-on, you're going to have to log and study a whole lot more data than if the pose is "closed form." (For simple time-dependent systems, storing just the output state of time T-1, and the new input state, may be enough to reproduce movement -- consider this for your system.)

Another option is to say "the system is weak enough to never hurt humans, and it's only for cosmetic display, so if it breaks, there's no money on the line." That's a fine way of doing systems analysis. In the past, I've designed systems with MX servos, that, because of a bug, moved into interference (stalled against themselves,) and before I could cut power (a few seconds,) the delicate Maxxon motors in those servos had already burned out their brushes, necessitating a $90 replacement for each servo motor. There is an "overheating" and "stalled movement" safety in the controller for the servo, but the timing for those safeties is 5-7 seconds, and the timing for burning out a motor at the highest rated voltage of 4S LiPos is more like 2 seconds... So, validation is not just about keeping humans safe, it's also about saving money. To avoid this, you'll have to guarantee that no link in your arm will interfere with (be blocked by) the body of the robot, or another link of the arm, when you do the iterative solution. When using a closed form solution, you can bake that guarantee into the solution, and rest assured that there will never be such a problem. All in all, iterative solutions are SO MUCH SIMPLER, and also often "look better" aesthetically, so many solutions end up doing just that, and living with the, now obvious, draw-backs related to validation :-) (Please accept this answer if you find it to be helpful.) • Hi Jon, thank you for your answer, actually I will make a choice based on the last known pose and the first experiments look good (so far :) ) but may be I missed something regarding the "obvious drawback"? Can you provide more details if you have time? You are right, there is an AX12 in the elbow and I am not planning to lift great weight - it is more about movement and body expression. – fabrice Mar 28 '17 at 9:30 • Thank you very much for your detailed answer Jon, I got the point! – fabrice Mar 31 '17 at 7:29 You can use differential IK to solve for a set of IK solutions along a given trajectory. The idea is that you use the IK solution of the current time step, to compute an IK solution for the next time step. Suppose$q$is your IK solution at the current time step and$x^+$is the desired end-effector pose at the next time step. You may compute$q^+$by iteratively updating$q$: $$q \leftarrow q + dq,$$ where$dq = J^{\dagger}(q)dx$,$J^{\dagger}$is a pseudo inverse of the Jacobian matrix, and$dx$is the difference between the current end-effector pose and the desired one. If a solution exists, this procedure should converge in only a few iterations (e.g. less than$10$iterations). The converged value is taken as$q^+\$.

An advantage of this method is that in the end you will have a smooth joint trajectory.

By the way, to select the initial IK solution for the trajectory, you may choose one which is well away from the configuration space boundary so that you wouldn't hit a boundary while following the end-effector trajectory.