Get the arm joint angles with kinect 1

Question

I am Carlos Barreiro and I am studying a Robotics Master. Now I am working with my Thesis. The project consists in the teleoperation of the a robot with Kinect (model 1). More specifically, I am working with the humanoid robot Pepper which is developed by Aldebaran (Softbank).

For the skeleton tracking in real time I am using the Kinect for Windows SDK (v1.8). Because with the kinect 1 I can’t obtain the Skeleton tracking in Linux :-(. It is not a problem, I think that I can continue with Windows for a few time.

For this project I use Python because I have more experience with this language and it is easier communicate with Naoqi (the robot middleware). For the communication with Kinect I am using the library PyKinect, it is a wrapper from the C++ Windows SDK.

My problem is calculating the rotation angle for the arm actuators. The robot needs translate the positions of the points of the skeleton to an angle for each motor (Pitch Roll or Yaw) like the bellow picture. So I need to get the shoulder pitch, should roll, elbow roll and elbow Yaw.

The skeleton that I got from Kinet gives me the 3d point and the quaternions of each joint (shoulder, elbow, …)
I am trying different ways but the result could be improved a lot of.

• CASE A:

I am using the joint 3D position for calculate the angle between two points.
For example :

The shoulder robotics has two actuators: the shoulder Roll and the Shoulder Pitch.

In this function takes I pass two arguments, in this case the 3D position point of the shoulder and the elbow and then I calculate the angle between each axis. Code:

def angulosXplano (puntoA, puntoB):

def calcularAngulo(uno, dos):
    # Compute the angle
    rads = math.atan2(-dos,uno)
    rads %= 2*math.pi
    degs = math.degrees(rads)
    return degs

dx = puntoB.x - puntoA.x
dy = puntoB.y - puntoA.y
dz = puntoB.z - puntoA.z

yaw  = calcularAngulo(dx, dy)
roll = calcularAngulo(dy, dz)
pitch = calcularAngulo(dx, dz)

For calculating the shoulderPitch angle of the robot I use the roll angle (that I got from the fuction angulosXplano) and for the shoulderRoll I use the pitch angle.

The reason of calculating the angles in this way is because I get better results than the results obtained if I calculate the shoulderPitch with the pitch angle and the shoulderRoll with the roll angle.

The angular movement for the shoulder roll is good, but the should pitch is moderate and the roll elbow movement is the worst because the shoulder movement affects to the elbow movement.

• CASE B:

Also I try to get the shoulder pitch and roll and the elbow yaw and roll with the quaternions of the SDK of the Microsoft.

In this case I tried the quaternions for the elbow, I obtain the quaternion like:

data.calculate_bone_orientations()[JointId.WristRight].hierarchical_rotation.rotation_quaternion

For the elbow I use the Wrist-Right because I read that the position of the joint depends on the previous joint.

After obtain this quaternion, I convert it to Euler angles, the next code it’s a method of a class that I had developed. The class has got the qw, qx, qy and qz quaternions params.

def quaternion2euler(self):
    q = self
    qx2 = q.x * q.x
    qy2 = q.y * q.y
    qz2 = q.z * q.z

    test = q.x*q.y + q.z*q.w

    if (test > 0.499):
        roll    = math.radians(360/math.pi*math.atan2(q.x,q.w))
        pitch = math.pi/2
        yaw     = 0
    elif (test < -0.499):
        roll    = math.radians(-360/math.pi*math.atan2(q.x,q.w))
        pitch = -math.pi/2
        yaw     = 0

    else:
        roll = math.atan2(2 * q.y * q.w - 2 * q.x * q.z, 1 - 2 * qy2 - 2 * qz2)
        pitch = math.asin(2*q.x*q.y+2*q.z*q.w)
        yaw = math.atan2(2*q.x*q.w-2*q.y*q.z,1-2*qx2-2*qz2)

    return [roll, pitch, yaw]

If use the yaw for the robot elbow roll, the results for the elbow roll improve a lot compared to the previous method. But I can’t find the angle for the robot elbow yaw.

• FUTURE CASE C:

My next step would be try the case A but with vectors instead of 3D points. For example the vector A (middle-Shoulder and right-Shoulder) and the vector B (right-Shoulder and elbow). But I have not developed anything yet.

Any help that can help me to improve the code, some bibliography or any better idea would be welcome.

Answer 1

Probably too late to help, but I think the problem is that there are not unique solutions to achieve some position. There are probably better practices, but you might try to search for a solution to a given position problem using a genetic algorithm and picking a candidate solution that gets close enough to the positional error you are willing to tolerate.