Robobuilder Controlled by Kinect

[Kondo]

Hi PedroR

It's fun, it's a luxury to prove oneself direct interaction Kinect sensor and a humanoid robot, the truth at first I had some small problem because the data rate of speed serial-usb adapter, and that to which the whole system collapsed.

Since everything is solved by configuring the serial cable of 9600 bps to 128000 bps (with the Robonova and the Kondo KHR2 I spend the same with their respective programs), and the whole is really stable and I have not have any problems with the programs necessary to operate the Kinect and RoboBuilder.

regards

[l3v3rz]

Had to go out and buy a Kinect ! (I got the Xbox 360 Version) Here's my version running. I modified the "Shape Game" demo from the Kinect SDK with C# version of your code - and here's the result :
<object width="420" height="315">
<param name="movie" value="http://www.youtube.com/v/jrFZDG9R-E0?version=3&amp;hl=en_US&amp;rel=0"></param>
<param name="allowFullScreen" value="true"></param>
<param name="allowscriptaccess" value="always"></param>
<embed src="http://www.youtube.com/v/jrFZDG9R-E0?version=3&amp;hl=en_US&amp;rel=0" type="application/x-shockwave-flash" width="420" height="315" allowscriptaccess="always" allowfullscreen="true"></embed>
</object>

Code: Select all

         void UpdateRobobuilderArms(Byte bServo11, Byte bservo14)
         {
            //left arm
            Byte bServo10=127, bServo12=127;
            double dServo11  = Convert.ToDouble(bServo11);

            //               5E-05x3 - 0.0213x2 + 2.9618x - 46.011
            try
            {
                bServo10 = Convert.ToByte(0.00005 * Math.Pow(dServo11, 3) - 0.0213 * Math.Pow(dServo11, 2) + 2.9618 * dServo11 - 46.011);

                //             8E-05x3 - 0.0339x2 + 4.7877x - 108.15
                bServo12 = Convert.ToByte(0.00008 * Math.Pow(dServo11, 3) - 0.0339 * Math.Pow(dServo11, 2) + 4.7877 * dServo11 - 108.15);
            }
            catch (Exception)
            {
            }
            // right arm
            Byte  bServo13=127 , bServo15=127;
            try
            {
                double dServo14  = Convert.ToDouble(bservo14);

                //               5E-05x3 - 0.0176x2 + 2.0147x + 87.278
                bServo13 = Convert.ToByte(0.00005 * Math.Pow(dServo14,3)  - 0.0176 * Math.Pow(dServo14,2) + 2.0147 * dServo14 + 87.27);
                //               8E-05x3 - 0.0272x2 + 3.0598x + 24.756
                bServo15 = Convert.ToByte(0.00008 * Math.Pow(dServo14,3)  - 0.0272 * Math.Pow(dServo14,2) + 3.0598 * dServo14 + 24.756);

            }
            catch (Exception)
            {
            }
            SendArmPosition(bServo10, bServo11, bServo12, bServo13, bservo14, bServo15);
        }

        void SendArmPosition(Byte bServo10, Byte bServo11, Byte bServo12, Byte bServo13, Byte bServo14, Byte bServo15 )
        {
            try {
                if (!bRBConnected)
                     return;

                rbWCK.wckMovePos(10, bServo10, 1); // 3rd parameter is Torque 0~4: 0=maximum
                rbWCK.wckMovePos(11, bServo11, 1);
                rbWCK.wckMovePos(12, bServo12, 1);
                rbWCK.wckMovePos(13, bServo13, 1);
                rbWCK.wckMovePos(14, bServo14, 1);
                rbWCK.wckMovePos(15, bServo15, 1);
            }
            catch(Exception e)
            {
                MessageBox.Show("Unable to Update Servo Position " + e.Message);
            }
        }

        int ScaleTo(double val, int imin, int imax, double dmin, double dmax)
        {
            int r;
            if (val <dmin> dmax) val = dmax;
            val = (val - dmin) / (dmax - dmin);
            r=  (int)((double)(imax - imin) * val) + imin;
            return r;
        }

        void updaterobot(Microsoft.Kinect.Skeleton playerSkeleton)
        {
            //Figure out values for the shoulder based on Hand position.
            // Kinect returns positive values if the hand is above the head and negative values if it's bellow the head
            int ServoRange  = bMaxShoulder - bMinShoulder;

            //Left Arm
            //Dim scaledLeftArm = playerSkeleton.Joints(JointType.HandRight).ScaleTo(1, ServoRange, 0.5F, 0.3F);
            int sla = ScaleTo(playerSkeleton.Joints[JointType.HandRight].Position.Y, 0, ServoRange, 0.0, 0.3);
            Byte bServo11 = Convert.ToByte(255 - (ServoRange - sla + bMinShoulder));

            // Right Arm
            //Dim scaledRightArm = playerSkeleton.Joints(JointType.HandLeft).ScaleTo(1, ServoRange, 0.5F, 0.3F);
            int sra = ScaleTo(playerSkeleton.Joints[JointType.HandLeft].Position.Y, 0, ServoRange, 0.0, 0.3);
            Byte bServo14 = Convert.ToByte(255-(255 - (ServoRange - sra + bMinShoulder)));

            UpdateRobobuilderArms(bServo11, bServo14);
        }

        void ConnectToRB(string sCOMMNum)
        {
            try {
                SerialPort s = new SerialPort(sCOMMNum,115200);
                s.Open();
                rbPCR = new RobobuilderLib.PCremote(s);
                rbWCK = new RobobuilderLib.wckMotion(rbPCR);

                if (rbWCK.wckReadPos(30))
                {
                    //DCMP is true
                    rbWCK.DCMP = true;
                    rbWCK.PlayPose(1000, 10, wckMotion.basic16, true);
                }
                else
                {
                    //DCMP false
                    rbPCR.runMotion(7); // basic posture
                    Thread.Sleep(3000);
                    rbWCK.DCMP = false;
                    rbPCR.setDCmode(true);
                }
                bRBConnected = true;
            }
            catch(Exception e)
            {
                MessageBox.Show("Unable to Connect to Robobuilder\n\n" + e.Message);
                rbPCR = null;
                bRBConnected = false;
            }
        }

        void DisconnectFromRB()
        {
            if (rbWCK != null)
                rbWCK.close();

            if (rbPCR != null)
                rbPCR.Close();

            rbWCK = null;
            rbPCR = null;
            bRBConnected = false;
        }


The COM port problem is caused by PCRemote trying to close the serial port with a null descriptor - it is bug - but the work round is to use the old style initialization, passing in the Serial port - and hence it won't be null.

I've also modified connect so it will work with either Standard firmware or DCMP firmware - it test servo 30.

Ive writen my own ScaleTo function so you don't need any extra library - just robobuilderlib.

[Kondo]

L3v3rz fantastic video, awesome!

By the way you play a game has come to Game Shape?

[l3v3rz]

Thanks.
I've commented out the falling shapes but you could leave in if you want.

I added the RBConnect/Disconnect code into the speech recognizer - on the word "Reset". That's how I control the Disconnect remotely at the end.

Code: Select all

void RecognizerSaidSomething(object sender, SpeechRecognizer.SaidSomethingEventArgs e)
{
            string txt = e.Matched;
            switch (e.Verb)
            {
                case SpeechRecognizer.Verbs.Reset:
                    if (bRBConnected)
                    {
                        txt = "Disconnect";
                        DisconnectFromRB();
                    }
                    else
                    {
                        txt = "Connecting ...";
                        ConnectToRB("COM4");
                    }
                    break;
            }
            FlyingText.NewFlyingText(this.screenRect.Width / 30, new Point(this.screenRect.Width / 2, this.screenRect.Height / 2), txt);
}

Also added into function private void SkeletonsReady(object sender, SkeletonFrameReadyEventArgs e) the line to actually make robot move. (But only if you're connected.

Code: Select all

if (bRBConnected)
      updaterobot(skeleton);


In the "using" section need to add (as well as in Reference section)

Code: Select all

using RobobuilderLib;


and I also added at the top of the code in the "private state" area

Code: Select all

        RobobuilderLib.wckMotion rbWCK = null;
        RobobuilderLib.PCremote rbPCR = null;
        Boolean bRBConnected = false;

        //Define maximum thresholds for relating Kinect to Shoulder
        Single sMaxKinectHandY = (Single)0.25;
        Byte bMaxShoulder = 185;

        Single sMinKinectHandY  = (Single)0.7;
        Byte bMinShoulder  = 47;


[PedroR]

Hi l3v3rz

Very nice!

I wanted to ask you if you've had issues with fluidity of the UI?

On some computers we've seen that the UI (in our version will the colored balls) would seem to freeze (and unfreeze once the skeleton was lost).

Other computers didn't seem to exhibit this behavior and your sample also looks very fluid.


I also wanted to tahnk you for clarifying the work around fort he COMM port issue.

We'll make sure to incorporate it in future revisions of the code.

Very nice work indeed (as always ) !

Regards

[l3v3rz]

I didn't have any problems with fluidity - I did cut the shape code out first and remove collision detection. My PC's not that fast either - 2.3GHz AMD 64 X2 (4400) with 2GB of RAM.

[PedroR]

Thanks l3v3rz

Limor has heard from WillowGarage that the Kinect bombards the USB host with data (indeed the code seems to be firing the event of SkeletonReady very quickly)

The issues we've had with fluidity of the UI were on a mobile Core i5 but from your report (and also our experience with other computers with less horsepower) it seems the issue is not consistently reproduced.

Anyway it is not really a big issue; just curious. Maybe I instaled too many Betas and demos for Kinect or maybe it boils down to the USb host controller on each computer.


We're on a very tight schedule of exhibitions and events so we don't have much time to evolve our Kinect demo.

One of the things we'd like to do was true inverse Kinematics on the upper body so that it could distinguish a punch (where you raise the arm and stretch it forward) from simply opening your arms.

At the Big Bang event the people were fascinated by the Robobuilder + Kinect and after early experiments they tried punching and other more complex movements (probably inspired by the the movie - "Real Steel") However the current code isn't able to distinguish different types of arm movements and the Robot couldn't keep up with them.

Regards
Pedro.

[Kondo]

Hello PedroR

One question, I have a lot couriosidad to know if the same environment that you have used with RoboBuilder and Kinect, it could transfer the Robonova robots and Kondo-KHR2?

[PedroR]

Hi Kondo

In theory it should be possible to use the same environment on other Robots.

The only part you would need to modify is the Sequence of Bytes that are sent to the Robot after processing the skeleton and determining how you want to position the arms.

In the case of robobuilder there is a well documented protocol to control the position of the servos individually using the Serial Port.

I don't know enough of Robonova and KHR 2Hv to be able to tell right away what you need to do but as long as they can receve commands over the Serial port to control the position of the servos, it should work.

Regards
Pedro

[MarcoP]

Hi

We have updated this project to include Full Inverse Kinematics for the Upper Torso.

New thread with the software for this Version 2.0 is here http://robosavvy.com/forum/viewtopic.php?p=34624

Regards

Marco