Kondo KHR3 Animation using Blender

[amuthelet]

Hi,

Starting from chrisvo's libkondo4, I'm working on controling my beloved KHR3 through a rigged 3D character from Blender (www.blender.org). The target is to be able to animated my robot using standard 3D character animation techniques, to ease the process compared to HTH. I would also like to experience blender's game engine and integrated real time physics engine to provide user friendly robot animation package.

It is on good track..but )

- I use Blender v2.49 / Python 2.6.
- I created my set of test bones that roughly describe (hmmm ..very roughly ;o)) my robot skeleton, with Inverse Kinematix and angle constraints.
- For each bone, I store corresponding kondo servo_id, rotation axis and rotation sign inside the bone name (didn't find any easy way to store custom object attributes using Blender 2.49).
- My armature/character calls a python script each time armature data is modified (for instance each time a bone orientation is modified) . This is achieved through Blender' script links mechanism. Cool, very usefull.
- The python script basically calls kondo_send_ics_pos for each bone, after finding out the right servo id, axis and sign by parsing bone name.
- I couldn't find a way to call kondo_send_ics_pos from python (couldn't find a way to pass UCHAR mask[5] to libkondo4 from a python bindings / python list), so finally added to libkondo4 my own kondo_send_servo_pos function building servo mask from int instead of UCHAR mask[5].

My problem today is I have very poor performances calling kondo_send_ics_pos for each of my 17 servos at each frame (I experience about 500 ms between each refresh). I see different possible reasons for that:
- python bindings might be slow (I have to test a similar example in C++ to check that)..but I doubt this is the problem.
- my python code on top of libkondo might slows that down (strings op at each frame, I need to optimize that, etc..)

But if I unplug the robot, then all runs quite smoothly, so I suspect a bottleneck stands in the process of calling kondo_send_ics_pos 17 times at each frame.

Anyone already experienced such issu with libkondo4 ? I am not famliliar with serial communication..am I doing something wrong ? Any other way to send one packet with the info for all servos instead of sending 17 commands ?

I could share my ongoing project (just a start), if anyone interested.

Thanks !
Arnaud.

[SK]

Have you looked at http://wiki.blender.org/index.php/Robotics:Contents and http://wiki.blender.org/index.php/Robot ... OpenRobots ? There are some people/research institutions looking into using Blender for robotics in various ways.
I'm not sure that stuff is needed for your application, but having a look probably doesn't hurt.

[amuthelet]

Didn't know about that, thanks for the links ! Very interesting indeed.

[silveranim]

hi amuthelet
it sounds great, to control your khr-3 from Blender..
i´am very interested to get more information about your project!!

[amuthelet]

Hi Silveranim,

Here is a Blender script (python) I wrote to control my robot from blender character. It requires manual naming of each bone, to define servo ID + axis of rotation + rotation sign. I could send Blender project if you want, but it's still very very early stage...

#! /usr/bin/env python

#############
# Blender script writtten by Arnaud Muthelet / Nov 2010
# Contact: amuthelet at gmail dot com
# Prereq:
# Blender v2.49, Python 2.6, libkondo4 from Christopher Vo
# Desc:
# Interactive command of Kondo KHR3 servos via RCB4
# controller, given
# orientations of virtual robot bones.
# WARNINGS:
# SERVOS CAN BE DAMAGED IF FORCED TO INVALID VALUES,
# PLEASE CHECK DATA SENT TO SERVOS BEFORE PLUGGING YOUR
# ROBOT (verbose = 1).
#################################################

import Blender
import sys
import math
from pykondo import *

SERVO_NEUTRAL = 7500
SERVO_MAX = 10000
SERVO_MIN = 5000
SERVO_DIFF = 30

VERBOSE = 1
KONDO_PLUGGED = 0

def init_registry(arm):
values = Blender.Registry.GetKey('servo_values')
if values:
if VERBOSE:
print "REGISTRY entry 'servo_value' already exists"
else:
values = {}
pose = arm.getPose()
poseBones = pose.bones
staticBones = arm.getData().bones
for boneT in staticBones.items():
bone_name = boneT[0]
values[bone_name] = 0
Blender.Registry.SetKey('servo_values',values,0)
if VERBOSE:
print "REGISTRY entry 'servo_values' created"

def set_servo(ki, arm):

previousValues = Blender.Registry.GetKey('servo_values')

sepID = "_#"
sepAxis = "_A"
sepSign = "_S"

pose = arm.getPose()
poseBones = pose.bones
staticBones = arm.getData().bones
for boneT in staticBones.items():
bone_name = boneT[0]
bone = poseBones[bone_name]

idIndex = bone_name.find(sepID) + len(sepID)
if(idIndex > len(sepID)-1):
servo_id = int(bone_name[idIndex:idIndex+2])

axisIndex = bone_name.find(sepAxis) + len(sepAxis)
axis = bone_name[axisIndex]

signIndex = bone_name.find(sepSign) + len(sepSign)
sign = bone_name[signIndex]
signFactor = 1

if( sign == "P" ):
signFactor = -1

if( axis == 'X'):
servo_value = int(SERVO_NEUTRAL + signFactor * math.floor(SERVO_DIFF*bone.quat.toEuler().x))
elif (axis == "Y"):
servo_value = int(SERVO_NEUTRAL + signFactor * math.floor(SERVO_DIFF*bone.quat.toEuler().y))
elif(axis == "Z"):
servo_value = int(SERVO_NEUTRAL + signFactor * math.floor(SERVO_DIFF*bone.quat.toEuler().z))
else:
servo_value = int(SERVO_NEUTRAL + signFactor * math.floor(SERVO_DIFF*bone.quat.toEuler().y))
if VERBOSE:
print "ERROR: No Axis found in servo name, defaulted to Y"

previousValue = previousValues[bone_name]

if( (servo_value <SERVO_MAX> SERVO_MIN) & (previousValue != servo_value)):
kondo_set_servo_pos(ki,servo_id,servo_value)
if VERBOSE:
print bone_name, "ServoID:",servo_id, "Sign:",sign,"Value:",servo_value, "Axis:", axis
elif VERBOSE:
print bone_name, "did not move, or moved out of range"

previousValues[bone_name] = servo_value
Blender.Registry.SetKey('robot_data',previousValues,0)
elif VERBOSE:
print "ERROR: no ID found in", bone_name


if Blender.bylink:
# Initialize Kondo lib
ki = KondoInstance()
if KONDO_PLUGGED:
ret = kondo_init(ki)
if ret < 0:
print(ki.error)
if VERBOSE:
print "Kondo lib init done"
init_registry(Blender.link)
set_servo(ki, Blender.link)

ret = kondo_close(ki)
if ret < 0:
print(ki.error)

else:
print "MANUAL LAUNCH"
[/img]

[amuthelet]

aaarg cut & past lost indent, that's a problem with Python script
Anyway tell me if you're still interested, I could post that as standalone file + associated blender project.
Cheers,
Arnaud.

[silveranim]

hello Arnaud, cool, thanks a lot for your script..

yes, it would be nice, to post the files, "standalone" and" the blender project"!

how it works?? its possible for you to post a movie to show the interaction between blender and the KHR3



thanks
frederic

[amuthelet]

As of today, here is the rough process:

- Be under Linux (never succeeded to run libkondo under Windows, though I made some modifications to make it compile inside VisualStudio, but ..)
- Download Blender 2.49 (it won't work with 2.5 or later, as it switches to Python 3)
- Launch Blender (from Linux console, so you have my script's debug info printed somewhere)
- Load my blender biped character configured for use with script (bones specific names including servo IDs + other info)
- Open python script inside Blender
- Plug Kondo robot
- Execute Python Script inside Blender
- Then you can use standard Blender's bones manipulators to change virtual robot posture, then it will automatically send appropriate commands to real servos.

It is a bit jaggy though (manual manip cannot be smooth), so the best would be to do offline animation modification inside Blender, then PLAY animation asyncronously. This should work with existing code mod. little modifications.

Didn't do it yet, but starting from that it should be rather straightforward to read any existing Kondo anim, store it as Blender key frames anims, edit them inside Blender (well key frames are not very easy to edit, but..), then play them back to Kondo Robot.

Limitation of this is I don't know how to store an animation inside RCB4 controller (chrisvo, if you can hear us )). So no way to store a mofidied animation inside the robot, you must use blender for each playback.. A workaround is probably possible, as I could write an HTH XML format for the anim, but it would be much better to be able to directly upload anim from libkondo.

Anyway you have to be VERY careful: manually manipulating bones with Blender manipulators is not always very smooth, and it's easy to send violent commands to servos (-> not good for servo lifetime, and even worth for anyone holding the robot ;o)).

I will create an archive of all that when I get home later today. Don't have any video, but I'll check if I can shoot something to post !
Cheers,
Arnaud.

[amuthelet]

Blender biped looks like that (bone scale are not even sized properly, as you can see ):

[amuthelet]

Hi silveranim,
Here is an archive of my Blender project:
http://robosavvy.com/Builders/amuthelet/BlenderKondo.zip

Are you already familiar with KHR3 robot control using libkondo library from chrisvo ? Are you already familiar with Blender ?

If not, then you will probably need more information than included in this email to get the whole stuff up and running.

The archives includes:
- blenderkondo_armature.blend: this is the blender project including the robot character with correctly named bones.
- blendkondo_control.py: this is the python scripts to be launch inside Blender to send commands to robot depending on character's current posture. This python script requires a little bit modified version of chrisvo's libkondo and its python bindings to be up and running on the target machine.
- rcb4.c, rcb4.h: this is just a sub-part of libkondo source I modified (well in fact I just added a new function called kondo_set_servo_pos, that wraps up kondo_send_ics_pos() function with a little bit higher level (allows to call the function directly with servo ID instead of rough bits). This is the function called in my blender's python scripts, so you need to replace those file in the original libkondo lib.

Tell me if you need more information about that.
Cheers,
Arnaud.

[silveranim]

hi Arnaud,
thanks a lot for your projekt files.
no, i´am not familiar with libkondo and blender,í´am just using Softimage XSI for character animations.
I'll try to use libkondo and blender...

my khr3 has three more servos for the hip, and body..I hope that I can integrate that..

best wishes
frederic

[silveranim]

hi Arnaud...
Now I have Blender 2.46 and Python 2.6 on a windows xp notebook.
Unfortunately, I have an unknown file type error when i open your project file in blender
perhaps a unix problem??

best wishes
frederic

[amuthelet]

Hi Frederic,
Project has been saved using Blender 2.49b, so you need this version to load it.

http://www.blender.org/development/release-logs/blender-249/

Blender projects are cross-platform files I presume, so you should be able to open a file saved under Linux using Windows XP.

Troubles will come when you want to run python script, as you will need to recompile libkondo + python bindings for Windows ... which will require a bit of technical work here I'm afraid.

Cheers,
Arnaud.

[amuthelet]

If I remember well, Softimage does support Python scripting. So that should feasible to do drive Kondo from a Softimage character as well.
Just have to figure out how to parse character bones from a Softimage biped using Python, and be sure a python script can run "forever as background task" inside Softimage. If I find some time I may have a quick look at that. Can you run Softimage under Linux ?