Transforming the Kinect coordinate system to the XR-4 coordinate system
The placement and securement of the Kinect with regards to the XR-4 is very important.
Position the Kinect (I used about 0.9 m from the base of the Arm) so that it is directly facing the XR-4 as shown in the pictures. The Kinect should be a minimum of 0.62 meter away from your closest object/target. If you can, secure the Kinect down to the table (I used double sided tape, which works very well) and set it at the desired angle (Ensure that the top of the Kinect is not tilted toward one side and is as parallel to the ground as possible.) You will also need to record the angle, with respect to ground, that you set the Kinect at. For your convenience I have included the following approximate angles that relate to their respective notches, that have held true in my experience. Each notch refers to an audible click that holds the Kinect at that position, when you manually adjust the angle. You can also calculate the angle through any other method of your choosing. From this point forward, you do not want to move or re-angle the Kinect again.
Position the Kinect (I used about 0.9 m from the base of the Arm) so that it is directly facing the XR-4 as shown in the pictures. The Kinect should be a minimum of 0.62 meter away from your closest object/target. If you can, secure the Kinect down to the table (I used double sided tape, which works very well) and set it at the desired angle (Ensure that the top of the Kinect is not tilted toward one side and is as parallel to the ground as possible.) You will also need to record the angle, with respect to ground, that you set the Kinect at. For your convenience I have included the following approximate angles that relate to their respective notches, that have held true in my experience. Each notch refers to an audible click that holds the Kinect at that position, when you manually adjust the angle. You can also calculate the angle through any other method of your choosing. From this point forward, you do not want to move or re-angle the Kinect again.
Next, open and run the downloaded MarkerLocation Simulink Model in MatLab. Ignore the point and coordinates that appear on the ImageViewer.
Now, place several different objects (at least 6, I used 6 Markers) into the view of the Kinect and within grabbing distance of the XR-4. The more objects that you use, in more locations, the more accurate your transformation will be. Then click pause on the Simulink model. On the Tools bar in the Image Viewer, click Pixel Region. Using this tool, identify the pixel coordinates of the top of each object. Make sure to select pixels slightly below the top edge of each object. Record these pixel locations. Then use these pixel coordinates as indices in the X,Y, and Z matrices within the MatLab function of the Simulink model, to determine X,Y, and Z. This will provide the (X,Y,Z) location of the object with respect to the Kinect's coordinate system. Note: For the Kinect coordinate system, the Y-axis is considered the vertical axis and the Z-axis is considered the depth axis (imagine the axis is pointing out of the lens of the Kinect).
Next, open and run the downloaded file labeled init.m. This will initialize the connection between the computer and the XR4 (Make sure the Mark IV Controller is turned on). Then use the movexyz.m file to move the robot gripper to each location by guessing and checking (The numbers you enter are in mm). Note: For the XR-4 coordinate system, the Z-axis is now considered the vertical axis and the Y-axis is now considered the depth axis.
Once you have the coordinates of the objects in (X,Y,Z) coordinates for both the Kinect and the XR-4, you can develop equations using Matrices and basic digital image processing to create the transformation equations for each axis. These equations will be able to transform any coordinate that the Kinect reads into the XR4's coordinate system. To do this, there will be several basic transformations that need to be performed. For a more complete understanding, please refer to pages 36-40 in "Digital Signal Processing" by Rafael C. Gonzalez and Paul Wintz. The equations that you will solve for will be of the following form:
Rx = Sx * ( x + xo )
Ry = Sy * ( y + yo ) * cos ( a ) + Sz * ( z + zo ) * sin ( a )
Rz = -Sy * ( y + yo ) * sin ( a ) + Sz * ( z + zo ) * cos ( a )
where
Rx, Ry, Rz= Robot coordinate
Sx, Sy, Sz = unknown scaling factor for the x, y and z axis
x,y,and z= the kinect coordinate
xo, yo, and zo= unknown displacement
a= the angle that the kinect is angled at
With the above equations, the angle of the Kinect, at least 6 kinect coordinates and their respective robot coordinates, the equations can be solved. This can be done either through MatLab, Mathematica, an advanced scientific graphing calculator (such as a TI-89), or any other method of your choosing.
You can then replace the equations in the MatLab function labeled TransformCoordinates with the new ones you calculated. Be sure to switch the y and z variables in the equations since these axes are switched between the Kinect and the Robot.
Defining a New Volume
Now, you can define a new volume of space that you'd like to use to detect the tallest object. To do so, replace the values in the MatLab code of the TransformCoordinates function in the Simulink model. These values are in mm and measured with respect to the origin of the XR4 coordinate axes. To determine the approximate location of the origin, you can use moveXYZ to move the robot to a known location and use that to determine the origin.
- Optional: You can move this part of the code in front of the transformation equations, with the necessary changes in variables. This way, only the isolated volume will be transformed. If you do this, the new isolated volume will be in meters and with respect to the Kinect coordinate system rather than that of the XR4. Note: I have not yet tried this.
Communicating Coordinates between the Kinect and Rhino XR-4
To communicate the coordinates of the highest point in the defined volume between the Kinect and the XR-4, open both the MarkerLocator Simulink model and the main.m file. Run the MarkerLocator and when the highest point is found on the object click pause. This saves the x,y,z data to the workspace. Now run the main.m file. The XR-4 will first reset to hard home and then the gripper will move to the object, pick it up, place it back down and finally return to its base. The command window will then prompt you if you would like to continue. To continue un-pause the simulink model, move your object, and ensure the top point of the object is being measured. If it is, again pause the simulation. Then type 'y' in the command prompt and hit enter. The XR-4 gripper will move to the new location of the object, pick it up, put it down, and again return to the base. When you are done type 'n' and hit enter, this will reset the XR-4 into hard-home and close the serial connection.