by PedroR » Wed Mar 14, 2012 5:50 pm
by PedroR
Wed Mar 14, 2012 5:50 pm
Hi all
Here comes the some hands-on information about our solution.
Firstly, this is how we designed the solution:
We started off with the demo code here
http://www.instructables.com/id/Kinect- ... g-Visual-/ which served as inspiration and kick starter.
(please note the code and instructions are outdated since Kinect SDK v1.0 has since been released and brought very significant changes to the Kinect classes, and the way we start and get skeletons from kinect)
This project itself is making use of one of the samples included in the Kinect SDK Beta 2. This is a Visual basic 2010 solution that runs under Visual basic Express (which is free).
All Visual basic samples have disappeared in the final V1.0 release.
1) What the sample does
What the VB Skeletal sample project does is make use of a Skeleton Tracking Sample that tracks the hands and head positions.
In the Instructables sample, the user has extended it to output a value through the COMM port between 0-180 to an Arduino that would in turn move 2 servos to animate a very simple 2 DoF figure.
He made use of a very cool extension from Coding4Fun for the Kinect which extends each joint class with a "ScaleTo" function.
This function takes the range to which you want to normalize the output (in his case 0-180) and optionally the upper and lower boundaries (in case you want to trim or fine tune the normalization to a specific area of positions)
One cool thing about kinect that is worth mentioning is that all values are normalized in relation to the head I believe (maybe it's a whole body approach...).
This means that you always get the same result regardless of the distance you have from kinect.
2) Adapting to Robobuilder
translating this sample to Robobuilder involved 2 main steps.
Step a) consists in converting the Bytes that are being sent into proper wCK move commands
This one was easily sorted with l3v3rz excellent .Net library for Robobuilder.
- We create an instance of the Robobuilder class,
- open the COMM,
- set the Robot to the Home position
- and enter Direct Control mode.
- next create an instance of the wCK class (which encapsulates commands to talk directly to the servos) and
- from there we just call the functions to set position.
While this involves a complete rewrite of the code that talks to the COMM port is no major deal. Anyone familiar with the RBC and wCK protocol can do this in less than 10 minutes. (yes that's how nice the libraries are!)
Step b) involved mapping the Hand position (1 single value) into a movement of the 3 servos on each arm.
This revealed more complex that we initially anticipated.
From the first moment
we wanted to make the movement as natural and human like as possible (i.e. we didn't want a Gull wing effect with stretched arms moving up and down attempting lift off!)
The idea was to have the Robot move from its standing position (elbows slightly bent and pointing backwards) to an elevated position with the arms stretched.
For this reason mapping the Hand Position only to Servos 11 and 14 (that control elevation of the arms) was not going to work for us.
We also needed to move servos 10/12 and 13/15 to go from elbows pointing backwards to stretched arms and vice versa.
First we used Motion Builder in Catch and Play mode and capture the value of the servos in 3 key frames:
- Arms down (home position with elbows pointing backwards)
- Arms at shoulder level (completely stretched)
- Arms up (completely stretched)
From here, we used Excel to Build a Table with these positions. The idea is to.
- map the Position of servo 10/12 in relation to the position of Servo 11
- map the position of servo 13/15 in relation to the position of servo 14
Table Discrete Position Progression For Raising Arm by
RoboSavvy, on Flickr
-
We also added some intermediate steps to define how we wanted the movement to progress. (these are the ones in black; you can see values progress much faster between the first 2 key frames)
This is
especially important to add the "Humanizing Factor": we wanted the arms to stretch and the shoulder to rotate significantly in the very early steps and then stay stretched until the arms are fully up.
We did not want a direct proportion between the positions of the servos as it would make it look too Robotic.
Once we had our table with the Discrete Positions set up,
we built an XY Scatter Chart with these values. We then used a cool tool in Excel (that Marco explained to me!) called TrendLine and selected Polynomial, 3rd degree.
Graph - Determining 3rd Degree Functions for Mapping Shoulder and Elbow Position to Servo 11 (raise of the arms) by
RoboSavvy, on Flickr
This gave us the position curves (and equations) for servo 10/13 that adjusts to the discrete positions on the table/graph.
The function relates the target servo position (10/12) to the position of servo 11.
(fyi the steps to do this in XL 2007 are: create the Chart (it MUST be set to "X Y (Scatter)"), select the "Layout" tab and click "Trendline". next click "more trendline options" and choose Polynomial).
We now have the functions to move our servos in a smooth, more natural way.
You may be wondering about servos 13/14/15; in practice if you want to mirror the movements just copy the table and calculate 255-[current value] to give you the mirrored position. next follow the exact same procedure.
It was now simply a matter of porting the functions to the Visual basic 2010 code.
The final code is quite simple.
To determine the Position of Servo 11 and 14 (which are needed to call
UpdateRobobuilderArms) we use the following code to directly relate the hand position to Servo position:
- Code: Select all
Dim bServo11 As Byte
Dim bServo14 As Byte
Dim ServoRange As Integer = bMaxShoulder - bMinShoulder
' Left Arm
Dim scaledLeftArm = playerSkeleton.Joints(JointType.HandRight).ScaleTo(1, ServoRange, 0.5F, 0.3F)
bServo11 = CByte(255 - (255 - (ServoRange - scaledLeftArm.Position.Y + bMinShoulder)))
' Right Arm
Dim scaledRightArm = playerSkeleton.Joints(JointType.HandLeft).ScaleTo(1, ServoRange, 0.5F, 0.3F)
bServo14 = CByte(255 - (ServoRange - scaledRightArm.Position.Y + bMinShoulder))
UpdateRobobuilderArms(bServo11, bServo14)
As you can see we use the "
ScaleTo" function from the Coding4Fun Kinect library to directly map hand position to Servo 11 and Servo 14 positions
Next
UpdateRobobuilderArms uses the functions we got from Excel to determine the position of the remaining servos (10/12 and 13/15) and uses the .Net library by l3v3rz to send the updated positions to the servos.
- Code: Select all
Private Sub UpdateRobobuilderArms(ByVal bServo11 As Byte, ByVal bservo14 As Byte)
' left arm
Dim bServo10 As Byte
Dim bServo12 As Byte
Dim dServo11 As Double = CDbl(bServo11)
' 5E-05x3 - 0.0213x2 + 2.9618x - 46.011
bServo10 = CByte(0.00005 * dServo11 ^ 3 - 0.0213 * dServo11 ^ 2 + 2.9618 * dServo11 - 46.011)
' 8E-05x3 - 0.0339x2 + 4.7877x - 108.15
bServo12 = CByte(0.00008 * (dServo11 ^ 3) - 0.0339 * (dServo11 ^ 2) + 4.7877 * dServo11 - 108.15)
' right arm
Dim bServo13 As Byte
Dim bServo15 As Byte
Dim dServo14 As Double = CDbl(bservo14)
' 5E-05x3 - 0.0176x2 + 2.0147x + 87.278
bServo13 = CByte(0.00005 * (dServo14 ^ 3) - 0.0176 * (dServo14 ^ 2) + 2.0147 * dServo14 + 87.27)
' 8E-05x3 - 0.0272x2 + 3.0598x + 24.756
bServo15 = CByte(0.00008 * (dServo14 ^ 3) - 0.0272 * (dServo14 ^ 2) + 3.0598 * dServo14 + 24.756)
SendArmPosition(bServo10, bServo11, bServo12, bServo13, bservo14, bServo15)
End Sub
Private Sub SendArmPosition(ByVal bServo10 As Byte, ByVal bServo11 As Byte, ByVal bServo12 As Byte, ByVal bServo13 As Byte, ByVal bServo14 As Byte, ByVal bServo15 As Byte)
On Error GoTo ErrHandler
If Not bRBConnected Then Exit Sub
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)
Exit Sub
ErrHandler:
MsgBox("Unable to Update Servo Position" & vbCrLf & vbCrLf & Err.Description, vbCritical And vbOKOnly, "Error Connecting")
Err.Clear()
End Sub
I am really anxious to get the Kinect and Robot back in the Office to make a Video that shows all of this coming together!
I will be posting the Source code, a compiled executable and installation instructions on the next post.
Please feel free to post your comments.
Regards
Pedro.
Hi all
Here comes the some hands-on information about our solution.
Firstly, this is how we designed the solution:
We started off with the demo code here
http://www.instructables.com/id/Kinect- ... g-Visual-/ which served as inspiration and kick starter.
(please note the code and instructions are outdated since Kinect SDK v1.0 has since been released and brought very significant changes to the Kinect classes, and the way we start and get skeletons from kinect)
This project itself is making use of one of the samples included in the Kinect SDK Beta 2. This is a Visual basic 2010 solution that runs under Visual basic Express (which is free).
All Visual basic samples have disappeared in the final V1.0 release.
1) What the sample does
What the VB Skeletal sample project does is make use of a Skeleton Tracking Sample that tracks the hands and head positions.
In the Instructables sample, the user has extended it to output a value through the COMM port between 0-180 to an Arduino that would in turn move 2 servos to animate a very simple 2 DoF figure.
He made use of a very cool extension from Coding4Fun for the Kinect which extends each joint class with a "ScaleTo" function.
This function takes the range to which you want to normalize the output (in his case 0-180) and optionally the upper and lower boundaries (in case you want to trim or fine tune the normalization to a specific area of positions)
One cool thing about kinect that is worth mentioning is that all values are normalized in relation to the head I believe (maybe it's a whole body approach...).
This means that you always get the same result regardless of the distance you have from kinect.
2) Adapting to Robobuilder
translating this sample to Robobuilder involved 2 main steps.
Step a) consists in converting the Bytes that are being sent into proper wCK move commands
This one was easily sorted with l3v3rz excellent .Net library for Robobuilder.
- We create an instance of the Robobuilder class,
- open the COMM,
- set the Robot to the Home position
- and enter Direct Control mode.
- next create an instance of the wCK class (which encapsulates commands to talk directly to the servos) and
- from there we just call the functions to set position.
While this involves a complete rewrite of the code that talks to the COMM port is no major deal. Anyone familiar with the RBC and wCK protocol can do this in less than 10 minutes. (yes that's how nice the libraries are!)
Step b) involved mapping the Hand position (1 single value) into a movement of the 3 servos on each arm.
This revealed more complex that we initially anticipated.
From the first moment
we wanted to make the movement as natural and human like as possible (i.e. we didn't want a Gull wing effect with stretched arms moving up and down attempting lift off!)
The idea was to have the Robot move from its standing position (elbows slightly bent and pointing backwards) to an elevated position with the arms stretched.
For this reason mapping the Hand Position only to Servos 11 and 14 (that control elevation of the arms) was not going to work for us.
We also needed to move servos 10/12 and 13/15 to go from elbows pointing backwards to stretched arms and vice versa.
First we used Motion Builder in Catch and Play mode and capture the value of the servos in 3 key frames:
- Arms down (home position with elbows pointing backwards)
- Arms at shoulder level (completely stretched)
- Arms up (completely stretched)
From here, we used Excel to Build a Table with these positions. The idea is to.
- map the Position of servo 10/12 in relation to the position of Servo 11
- map the position of servo 13/15 in relation to the position of servo 14
Table Discrete Position Progression For Raising Arm by
RoboSavvy, on Flickr
-
We also added some intermediate steps to define how we wanted the movement to progress. (these are the ones in black; you can see values progress much faster between the first 2 key frames)
This is
especially important to add the "Humanizing Factor": we wanted the arms to stretch and the shoulder to rotate significantly in the very early steps and then stay stretched until the arms are fully up.
We did not want a direct proportion between the positions of the servos as it would make it look too Robotic.
Once we had our table with the Discrete Positions set up,
we built an XY Scatter Chart with these values. We then used a cool tool in Excel (that Marco explained to me!) called TrendLine and selected Polynomial, 3rd degree.
Graph - Determining 3rd Degree Functions for Mapping Shoulder and Elbow Position to Servo 11 (raise of the arms) by
RoboSavvy, on Flickr
This gave us the position curves (and equations) for servo 10/13 that adjusts to the discrete positions on the table/graph.
The function relates the target servo position (10/12) to the position of servo 11.
(fyi the steps to do this in XL 2007 are: create the Chart (it MUST be set to "X Y (Scatter)"), select the "Layout" tab and click "Trendline". next click "more trendline options" and choose Polynomial).
We now have the functions to move our servos in a smooth, more natural way.
You may be wondering about servos 13/14/15; in practice if you want to mirror the movements just copy the table and calculate 255-[current value] to give you the mirrored position. next follow the exact same procedure.
It was now simply a matter of porting the functions to the Visual basic 2010 code.
The final code is quite simple.
To determine the Position of Servo 11 and 14 (which are needed to call
UpdateRobobuilderArms) we use the following code to directly relate the hand position to Servo position:
- Code: Select all
Dim bServo11 As Byte
Dim bServo14 As Byte
Dim ServoRange As Integer = bMaxShoulder - bMinShoulder
' Left Arm
Dim scaledLeftArm = playerSkeleton.Joints(JointType.HandRight).ScaleTo(1, ServoRange, 0.5F, 0.3F)
bServo11 = CByte(255 - (255 - (ServoRange - scaledLeftArm.Position.Y + bMinShoulder)))
' Right Arm
Dim scaledRightArm = playerSkeleton.Joints(JointType.HandLeft).ScaleTo(1, ServoRange, 0.5F, 0.3F)
bServo14 = CByte(255 - (ServoRange - scaledRightArm.Position.Y + bMinShoulder))
UpdateRobobuilderArms(bServo11, bServo14)
As you can see we use the "
ScaleTo" function from the Coding4Fun Kinect library to directly map hand position to Servo 11 and Servo 14 positions
Next
UpdateRobobuilderArms uses the functions we got from Excel to determine the position of the remaining servos (10/12 and 13/15) and uses the .Net library by l3v3rz to send the updated positions to the servos.
- Code: Select all
Private Sub UpdateRobobuilderArms(ByVal bServo11 As Byte, ByVal bservo14 As Byte)
' left arm
Dim bServo10 As Byte
Dim bServo12 As Byte
Dim dServo11 As Double = CDbl(bServo11)
' 5E-05x3 - 0.0213x2 + 2.9618x - 46.011
bServo10 = CByte(0.00005 * dServo11 ^ 3 - 0.0213 * dServo11 ^ 2 + 2.9618 * dServo11 - 46.011)
' 8E-05x3 - 0.0339x2 + 4.7877x - 108.15
bServo12 = CByte(0.00008 * (dServo11 ^ 3) - 0.0339 * (dServo11 ^ 2) + 4.7877 * dServo11 - 108.15)
' right arm
Dim bServo13 As Byte
Dim bServo15 As Byte
Dim dServo14 As Double = CDbl(bservo14)
' 5E-05x3 - 0.0176x2 + 2.0147x + 87.278
bServo13 = CByte(0.00005 * (dServo14 ^ 3) - 0.0176 * (dServo14 ^ 2) + 2.0147 * dServo14 + 87.27)
' 8E-05x3 - 0.0272x2 + 3.0598x + 24.756
bServo15 = CByte(0.00008 * (dServo14 ^ 3) - 0.0272 * (dServo14 ^ 2) + 3.0598 * dServo14 + 24.756)
SendArmPosition(bServo10, bServo11, bServo12, bServo13, bservo14, bServo15)
End Sub
Private Sub SendArmPosition(ByVal bServo10 As Byte, ByVal bServo11 As Byte, ByVal bServo12 As Byte, ByVal bServo13 As Byte, ByVal bServo14 As Byte, ByVal bServo15 As Byte)
On Error GoTo ErrHandler
If Not bRBConnected Then Exit Sub
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)
Exit Sub
ErrHandler:
MsgBox("Unable to Update Servo Position" & vbCrLf & vbCrLf & Err.Description, vbCritical And vbOKOnly, "Error Connecting")
Err.Clear()
End Sub
I am really anxious to get the Kinect and Robot back in the Office to make a Video that shows all of this coming together!
I will be posting the Source code, a compiled executable and installation instructions on the next post.
Please feel free to post your comments.
Regards
Pedro.