Kinect Angles Calculation Script

This is a brief explanation of code used to convert Kinect input from the Joints structure into the degrees of freedom of the AMIGO's arms.

This code is part of a project to create a robot technique that allows users to hug someone when separated by a disctance like a phone call.

For a mathmatical explanation of how the degrees of freedom are determined, check this section of the project.

The data calculated from the Kinect sensor's data is exported to a text-file, after which it is send to a Ubuntu machine controlling the AMIGO. The piece of code handling the outputting of data to a text file is found in the same code as featured on this page, but it is described here.

Vectors and Vector Manipulation

As c++ does not provide a standard library for 3 dimensional vectors and vector manipulations, we made our own Vector3 class that gives the user access to its coördinates and contains a function that returns the length of the vector. This code is not contained here but can be read in the full code that can be downloaded from this wiki page.

Several manipulations on 3 dimensional vectors can be found in the same class that also converts the Kinect input into useable data. These are:

• subtractV, a function that returns a Vector3 that is a difference vector pointing from one CameraSpacePoint to another CameraSpacePoint.
• angleV, a function that returns the angle between two vectors, calculated using the vector dot product.
• dotV, a function that returns the dot product between two vectors.
• crossV, a function that returns the cross product between two vectors.
• projectPlaneV, a function that returns the projection of a vector onto a plane given by that plane's normal vector.
• projectVectorV, a function that projects a vector onto another vector.
• normalizedV, a function that returns a vector with the same direction of the input vector, but with length one.
• converseV, a function that returns the converse of the input vector, the vector multiplied by -1.

Extracting Data from Kinect Input

The data needed is calculated from an array of Joint objects from the closest skeleton tracked by the Kinect sensor by the function GetArmAngles. It returns an 14 sized array of double's representing the 14 angles calculated, 7 per arm. Below and excerpt of this function is given in which the angles for the left arm are calculated. For an explanation, see the mathematical explanation mentioned before. Note that the vectors are given more human-readable names than the short ' a '. Vectors pointing from one joint to the other are often given a name using a standard format. For example, the vector sh2elL is the vector pointing from the left shoulder to the left elbow. The L is used to indicate that this vector belongs to the calculation of angles for the Left arm.

The first lines 730-733 create an empty array of 14 double's that will be filled in with the corresponding angles by the code to come.

The code starts by calcuating the second degree of freedom first. The first and second degree of freedom are closely related as they both require the vector pointing from the shoulder to the elbow to be projected onto the side of the player.

In this piece of the code, the first, third and fourth degree of freedom are calculated.

Above the fifth and sixth degree of freedom of the left arm are calculated.

In this final excerpt of code, the remainder of an attempt to calculate the final degree of freedom can be seen. As can be read in the full explanation, this calculation appeared to be flawed and no attempt was made to improve it as it was not extremely necessary for the project. Readers attempting a similar robot technique might use this code as inspiration or see it as a warning sign.