Quadruped Robot 4: Simulation on Blender

Quadruped Robot 4: Simulation on Blender

As a kid with a new toy, as soon as my twelve servos arrived I quickly began to assemble what I had. In no time I had a leg to test my code. And so I coded the basic hardware interface, and then the inverse kinematics.

Turns out it's not a very good idea to code and test this kind of system in real hardware, the outcome was, in the end, 5 broken servos due to stalling against the body itself.

So I decided to simulate it.

In the end I interface with two different programs idepently, Blender and V-Rep.

Blender

I had the knowledge that blender has a built-in game engine, with physics engine and everything, so why not use it? Doing some research I found a tutorial on how to use armatures here, so I imported my .stl files used for 3D printing into blender, and setup the armature, resulting in this:

alt text

I then wanted to control this armature using python. In order to use python you need to setup some game logic to something like this:

alt text

So that the "legs" module is imported on the beginning of the simulation, and the function "step" is called on each of the Game Engine simulation steps.

The "legs.py" file has to be created on Blender's scripting interface and is placed inside the .blend file, but it can import modules from outside, so you can use an external editor.

To control this armature from code, I did the following:

from robot.robotInterfaces.legInterfaces.genericLeg import Leg
import bge

scene = bge.logic.getCurrentScene()
source = scene.objects

class VirtualLeg(Leg):
    def __init__(self, name, position):
        Leg.__init__(self, name, position)
        self.armature = source.get("armature")
        self.channels = []
        for channel in self.armature.channels:
            if self.name in str(channel):
                self.channels.append(channel)
        self.direction = -1 if "right" in self.name else 1


    def move_to_angle(self, shoulderAngle, femurAngle, tibiaAngle):
        """
        Move legs joints to the specified angles, in radians.
        """
        self.check_limits(shoulderAngle,femurAngle,tibiaAngle)
        shoulder = self.armature.channels[0]
        shoulder.rotation_mode = bge.logic.ROT_MODE_XYZ
        shoulder.rotation_euler = (0, -shoulderAngle, 0)

        femur = self.armature.channels[2]
        femur.rotation_mode = bge.logic.ROT_MODE_XYZ
        femur.rotation_euler = (-femurAngle, 0, 0)

        tibia = self.armature.channels[3]
        tibia.rotation_mode = bge.logic.ROT_MODE_XYZ
        tibia.rotation_euler = (0, 0, -tibiaAngle*self.direction)

        self.armature.update()

What happens, from the beggining:

Here we get a "list" with all the objects in the scene.

source = bge.logic.getCurrentScene().objects

Then, we define the VirtualLeg class, a leg that exists only in Blender.

class VirtualLeg(Leg):
    def __init__(self, name, position):
        Leg.__init__(self, name, position)
            self.armature = source.get("armature")
            self.channels = []
            for channel in self.armature.channels:
                if self.name in str(channel):
                    self.channels.append(channel)
            self.direction = -1 if "right" in self.name else 1

Whenever a leg is created, it finds the armature object on the scene, and scans it its channels(a channels is how you control a joint) looking for the ones containing the leg's name on their own names(names can be seen on the armature structure on the first image), and add these chanels as it's own. It then checks if it's facing right, and sets a flag indicating so.

Then we define the VirtualLeg's moveToAngle() method:

    def move_to_angle(self, shoulderAngle, femurAngle, tibiaAngle):
        """
        Move legs joints to the specified angles, in radians.
        """
        self.check_limits(shoulderAngle,femurAngle,tibiaAngle)
        shoulder = self.armature.channels[0]
        shoulder.rotation_mode = bge.logic.ROT_MODE_XYZ
        shoulder.rotation_euler = (0, -shoulderAngle, 0)

        femur = self.armature.channels[2]
        femur.rotation_mode = bge.logic.ROT_MODE_XYZ
        femur.rotation_euler = (-femurAngle, 0, 0)

        tibia = self.armature.channels[3]
        tibia.rotation_mode = bge.logic.ROT_MODE_XYZ
        tibia.rotation_euler = (0, 0, -tibiaAngle*self.direction)

        self.armature.update()

This method transforms the control's angles into proper movement in the game engine. First it checks if the received angles are valid, then, for each of the joints, it sets the rotation modes and apply the angle with proper direction correction. Finally, we tell the armature to update on the screen with the new joint values.

The final result:

The final result, with my control code plugged in, is this:

Basic IK test:

Trot gait test:

The Downside

The problem I had with blender is that, if not impossible, getting the armature to work with the physiscs engine properly is incredibly complex.

Coming next: V-Rep as a simulator.