two legged walker

I'm trying to simulate the following creature. But it doesn't seem to go well. I haven't really found any tutorials for the programming of evolving creatures, so I tried to adjust the walker demo. Please have a look at the code and let me know how I can make it move.

Thanks,
Steffen

#
# Walker.tz
# jon klein
#
# Walker is an attempt to evolve locomotion behavior for a simple
# creature using a genetic algorithm. THIS DEMO MAY REQUIRE AT
# LEAST 30 MINUTES BEFORE WALKING BEHAVIORS ARE OBSERVED.
# It may be best to run this demo overnight. Additionally, it
# is possible that no evolution will occur during the course of
# a simulation, so if you don't see results after an hour or two,
# start the simulation over again so that a new random population
# is used. Remember that the behaviors are evolved starting with
# completely random individuals.
#
# The algorithm can be described as thirty monkeys attempting to learn
# to drive a bus--they take turns one at a time, breeding the best
# individuals together to replace the worst individuals.
#
# We place the physical creature in the world and then attempt to evolve
# individuals (non-physical objects in this simulation) which will control
# the physical creature.
#
# The algorithm is described in the steps below:
# 1) create a number of random individuals
# 2) pick four individuals at random
# 3) let each individual control the body and record how far it moves
# 4) sort the four best individuals based on which could go the furthest
# 5) breed the best two individuals, create two offspring which will
# replace the two worst individuals.
# 6) return to step 2 and repeat.
#
# The creatures quickly learn to move by dragging the body along the
# ground and after longer simulations, realistic "walking" has been
# observed.
#

@use PhysicalControl.
@use Link.
@use Shape.
@use Stationary.
@use MultiBody.
@use File.
@use Matrix.
@use Vector.

@define SPEED_K 10.

Controller Walker.

PhysicalControl : Walker {
+ variables:
seats, monkeys (list).

currentSeat (int).

walker (object).

counter (int).
fileName (string).
fileID (object).

# the following are flags that can be controlled by the user
# via the simulation menu

locked (int).

lockMenu, symMenu (object).

cloudTexture (object).

flag (object).

+ to init:
floor (object).
number (int).
item (object).

locked = 0.
counter = 0.
fileName = "links_v2_$counter".
print fileName.
fileID = new File.
fileID open-for-writing with-file fileName.

flag = new Mobile.
flag disable-physics.
flag set-shape to (new Sphere init-with radius .2).
flag set-color to (1, 0, 0).
flag set-label to "Start".

self set-random-seed-from-dev-random.

self enable-fast-physics.
self set-fast-physics-iterations to 50.

# these are the default values anyway...

self enable-lighting.
self enable-smooth-drawing.

self move-light to (0, 20, 0).

# Create the floor for the critter to walk on.

floor = new Floor.
floor catch-shadows.

cloudTexture = (new Image load from "images/clouds.png").

self enable-shadow-volumes.
# self enable-reflections.

self set-background-color to (.4, .6, .9).
self set-background-texture-image to cloudTexture.

# Create the Creature.

walker = new Creature.
walker move to (0, 6, 0).
self offset-camera by (13, 13, -13).
self watch item walker.

# The "Monkeys" are the individuals that will control the Creature.
# Create the Monkeys and assign them numbers.

monkeys = 15 new Monkeys.

foreach item in monkeys: {
(item set-number to number).
number += 1.
}

self pick-drivers.

# set up the menus...

lockMenu = (self add-menu named "Lock Driver" for-method "toggle-driver-lock").

self add-menu-separator.

self add-menu named "Save Current Genome" for-method "save-current-genome".
self add-menu named "Load Into Current Genome" for-method "load-into-current-genome".

# schedule the first driver change and we're ready to go.

self schedule method-call "change-drivers" at-time (self get-time) + 60.0.

self display-current-driver.

+ to get-fileID:
return fileID.

+ to display-current-driver:
currentNumber (int).

currentNumber = (seats{currentSeat} get-number).

fileID write-line text "current driver #$currentNumber".

self set-display-text to "Driver #$currentNumber" at-x -.95 at-y -.9.

+ to iterate:
parts (list).
angle (float).
i (int).
position (vector).
rotation (matrix).
print1 (float).
print2 (float).
print3 (float).
print4 (float).

seats{currentSeat} control robot walker at-time (self get-time).

############################################################################
# #
# Here is the writing of the joint information supposed to go #
# #
############################################################################

parts = (walker get-all-connected-links).
# print ("number of links :"+|parts|).
i = 0.

# for i = 0, i < 5, i+=1: {
# position = parts{i} get-location.
# fileID write-line text "link $i".

# rotation = parts{i} get-rotation-matrix.
# fileID write-line text "$rotation".
# print1 = rotation{0}{0}.
# print2 = rotation{1}{0}.
# print3 = rotation{2}{0}.
# fileID write-line text "$print1 $print2 $print3 0.000000".

# print1 = rotation{0}{1}.
# print2 = rotation{1}{1}.
# print3 = rotation{2}{1}.
# fileID write-line text "$print1 $print2 $print3 0.000000".

# print1 = rotation{0}{2}.
# print2 = rotation{1}{2}.
# print3 = rotation{2}{2}.
# fileID write-line text "$print1 $print2 $print3 0.000000".

# print1 = position{0}.
# print2 = position{1}.
# print3 = position{2}.
# fileID write-line text "$print1 $print2 $print3 1.000000".
# }

super iterate.

+ to pick-drivers:
# pick 4 new drivers at random. we do this by sorting the
# list randomly and picking the first 4 items.

sort monkeys with random-sort.

seats{0} = monkeys{0}.
seats{1} = monkeys{1}.
seats{2} = monkeys{2}.
seats{3} = monkeys{3}.

currentSeat = 0.

+ to random-sort objectA a (object) objectB b (object):
return random[3] - 1.

+ to change-drivers:
newDriver (int).
newOffset (vector).

# if they have locked the current driver, do nothing.

if locked: return.

# pick a new camera angle and pan...

newOffset = random[(30, 6, 30)] + (-15, 1, -15).
if |newOffset| < 14: newOffset = 14 * newOffset/|newOffset|.
self pan-camera-offset by newOffset steps 30.

# we change the drivers every time a monkey is finished it's
# turn. if we have seen the last monkey, breed them together.

seats{currentSeat} set-distance to |(walker get-location)|.
walker center.

currentSeat += 1.

if currentSeat > 3: {
counter++.
fileName = "links_v2_$counter".
print fileName.
fileID close.
fileID open-for-writing with-file fileName.

self breed-new-monkeys.
self pick-drivers.
}

newDriver = (seats{currentSeat} get-number).

# schedule a new driver change in 20 seconds.

self schedule method-call "change-drivers" at-time (self get-time) + 60.0.
self display-current-driver.

+ to breed-new-monkeys:
print "breeding monkeys...".

sort seats with compare-distance.

# print out the distance for each individual tested

print "driver ", (seats{0} get-number), (seats{0} get-distance).
print "driver ", (seats{1} get-number), (seats{1} get-distance).
print "driver ", (seats{2} get-number), (seats{2} get-distance).
print "driver ", (seats{3} get-number), (seats{3} get-distance).

# breed the two best twice, replacing the two worst.

seats{0} breed with seats{1} to-child seats{2}.
seats{1} breed with seats{0} to-child seats{3}.

# give each individual a mutation

(seats{2} get-genome) mutate.
(seats{3} get-genome) mutate.

+ to compare-distance of a (object) with b (object):
result (float).

result = (b get-distance) - (a get-distance).
return result.

# the following methods are accessed from the simulation menu.

+ to toggle-driver-lock:
if locked == 1: {
locked = 0.
walker center.
self schedule method-call "change-drivers" at-time (self get-time) + 20.0.
lockMenu uncheck.
} else {
locked = 1.
lockMenu check.
}

+ to save-current-genome:
(seats{currentSeat} get-genome) save-with-dialog.

+ to load-into-current-genome:
(seats{currentSeat} get-genome) load-with-dialog.
}

Object : Monkeys {
+ variables:
distanceTraveled (float).
genome (object).

number (int).

+ to set-number to n (int):
number = n.

+ to get-number:
return number.

+ to init:
genome = new MonkeyGenome.
self randomize.

+ to randomize:
genome randomize.

+ to get-genome:
return genome.

+ to breed with otherMonkey (object) to-child child (object):
(child get-genome) crossover from-parent-1 (otherMonkey get-genome) from-parent-2 (self get-genome).

+ to control robot theRobot (object) at-time t (float):
theRobot set-joint-velocity-1 to (genome calculate-torque-3 at t).
theRobot set-joint-velocity-2 to (genome calculate-torque-3 at t).
theRobot set-joint-velocity-3 to (genome calculate-torque-3 at t).
theRobot set-joint-velocity-4 to (genome calculate-torque-4 at t).
theRobot set-joint-velocity-5 to (genome calculate-torque-2 at t).
theRobot set-joint-velocity-6 to (genome calculate-torque-3 at t).

+ to set-distance to value (float):
distanceTraveled = value.

+ to get-distance:
return distanceTraveled.
}

Object : MonkeyGenome {
+ variables:
genomeData (list).

+ to init:
genomeData = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }.

+ to randomize:
genomeData{28} = random[5.0] - 2.5.

genomeData{0} = random[2.0] - 1.0.
genomeData{1} = random[2.0] - 1.0.
genomeData{2} = random[2.0] - 1.0.
genomeData{3} = random[2.0] - 1.0.
genomeData{4} = random[2.0] - 1.0.
genomeData{5} = random[2.0] - 1.0.
genomeData{6} = random[2.0] - 1.0.
genomeData{7} = random[2.0] - 1.0.
genomeData{8} = random[2.0] - 1.0.
genomeData{9} = random[2.0] - 1.0.
genomeData{10} = random[2.0] - 1.0.
genomeData{11} = random[2.0] - 1.0.
genomeData{12} = random[2.0] - 1.0.
genomeData{13} = random[2.0] - 1.0.

genomeData{14} = random[6.3] - 3.15.
genomeData{15} = random[6.3] - 3.15.
genomeData{16} = random[6.3] - 3.15.
genomeData{17} = random[6.3] - 3.15.
genomeData{18} = random[6.3] - 3.15.
genomeData{19} = random[6.3] - 3.15.
genomeData{20} = random[6.3] - 3.15.
genomeData{21} = random[6.3] - 3.15.
genomeData{22} = random[6.3] - 3.15.
genomeData{23} = random[6.3] - 3.15.
genomeData{24} = random[6.3] - 3.15.
genomeData{25} = random[6.3] - 3.15.
genomeData{26} = random[6.3] - 3.15.
genomeData{27} = random[6.3] - 3.15.

+ to get-value at-index n (int):
return genomeData{ n }.

+ to calculate-torque-1 at time (float):
vec (vector).
vec set-value to (SPEED_K * .5 * ( sin( genomeData{ 28 } * ( time + genomeData{ 14 } ) ) - genomeData{ 0 } )) at 0.
vec set-value to (SPEED_K * .5 * ( sin( genomeData{ 28 } * ( time + genomeData{ 15 } ) ) - genomeData{ 1 } )) at 1.
vec set-value to (SPEED_K * .5 * ( sin( genomeData{ 28 } * ( time + genomeData{ 16 } ) ) - genomeData{ 2 } )) at 2.
return vec.

+ to calculate-torque-2 at time (float):
vec (vector).
vec set-value to (SPEED_K * .5 * ( sin( genomeData{ 28 } * ( time + genomeData{ 17 } ) ) - genomeData{ 3 } )) at 0.
vec set-value to (SPEED_K * .5 * ( sin( genomeData{ 28 } * ( time + genomeData{ 18 } ) ) - genomeData{ 4 } )) at 1.
vec set-value to (SPEED_K * .5 * ( sin( genomeData{ 28 } * ( time + genomeData{ 19 } ) ) - genomeData{ 5 } )) at 2.
return vec.

+ to calculate-torque-3 at time (float):
return SPEED_K * .5 * ( sin( genomeData{ 8 } * ( time + genomeData{ 20 } ) ) - genomeData{ 6 } ).

+ to calculate-torque-4 at time (float):
return SPEED_K * .5 * ( sin( genomeData{ 8 } * ( time + genomeData{ 21 } ) ) - genomeData{ 7 } ).

+ to calculate-torque-5 at time (float):
vec (vector).
vec set-value to (SPEED_K * .5 * ( sin( genomeData{ 28 } * ( time + genomeData{ 22 } ) ) - genomeData{ 8 } )) at 0.
vec set-value to (SPEED_K * .5 * ( sin( genomeData{ 28 } * ( time + genomeData{ 23 } ) ) - genomeData{ 9 } )) at 1.
vec set-value to (SPEED_K * .5 * ( sin( genomeData{ 28 } * ( time + genomeData{ 24 } ) ) - genomeData{ 10 } )) at 2.
return vec.

+ to calculate-torque-6 at time (float):
vec (vector).
vec set-value to (SPEED_K * .5 * ( sin( genomeData{ 28 } * ( time + genomeData{ 25 } ) ) - genomeData{ 11 } )) at 0.
vec set-value to (SPEED_K * .5 * ( sin( genomeData{ 28 } * ( time + genomeData{ 26 } ) ) - genomeData{ 12 } )) at 1.
vec set-value to (SPEED_K * .5 * ( sin( genomeData{ 28 } * ( time + genomeData{ 27 } ) ) - genomeData{ 13 } )) at 2.
return vec.

+ to crossover from-parent-1 p1 (object) from-parent-2 p2 (object):
n, crossoverPoint (int).
tmp (object).

if random[ 1 ]: {
tmp = p2.
p2 = p1.
p1 = tmp.
}

crossoverPoint = random[ | genomeData | - 1 ].

for n = 0, n < | genomeData |, n++: {
if n < crossoverPoint:
genomeData{ n } = ( p1 get-value at-index n ).
else
genomeData{ n } = ( p2 get-value at-index n ).
}

+ to mutate:
n (int).

n = random[8].

if n < 4: genomeData{n} = random[2.0] - 1.0.
else if n < 8: genomeData{n} = random[6.3] - 3.15.
else genomeData{n} = random[5.0] - 2.5.

print "mutated item $n of $self".
}

MultiBody : Creature {
+ variables:
bodyLink (object).
links (list).
joints (list).

+ to get-root:
return bodyLink.

+ to get-joints:
return joints.

+ to init:
counter (int).
linkShape, lowerLinkShape, bodyShape, axis, footShape (object).

self add-menu named "Send to Center" for-method "center".

bodyShape = (new PolygonDisk init-with sides 64 height .35 radius .55 ).
axis = (new PolygonDisk init-with sides 32 height 1 radius .2 ).
linkShape = (new PolygonDisk init-with sides 32 height .55 radius .1 ).
lowerLinkShape = (new PolygonDisk init-with sides 32 height .6 radius .1 ).
footShape = (new Cube init-with size ( .4, .4, 1.2 )).

links = 7 new Links.
joints{0} = new FixedJoint.
joints{1} = new BallJoint.
joints{2} = new BallJoint.
joints{3} = new RevoluteJoint.
joints{4} = new RevoluteJoint.
joints{5} = new BallJoint.
joints{6} = new BallJoint.

links{0} set-shape to axis.
links{1} set-shape to linkShape.
links{2} set-shape to linkShape.
links{3} set-shape to lowerLinkShape.
links{4} set-shape to lowerLinkShape.
links{5} set-shape to footShape.
links{6} set-shape to footShape.

self add-dependency on joints{0}.
self add-dependency on joints{1}.
self add-dependency on joints{2}.
self add-dependency on joints{3}.
self add-dependency on joints{4}.
self add-dependency on joints{5}.
self add-dependency on joints{6}.

links set-color to (1.0, 1.0, 1.0).

bodyLink = new Link.
bodyLink set-shape to bodyShape.

joints{0} link
with-parent-point ( 0, 0, 0 )
with-child-point ( 0, 0, 0 )
to-child links{0}
parent bodyLink.

joints{1} link
with-parent-point ( 0, -.53, 0)
with-child-point ( 0, .305, 0)
to-child links{1}
parent links{0}.

joints{3} link
with-normal ( 1, 0, 0 )
with-parent-point (0, -.305, 0)
with-child-point (0, .33, 0)
to-child links{3}
parent links{1}.

joints{5} link
with-parent-point (0, -.33, 0)
with-child-point (0, .23, .35)
to-child links{5}
parent links{3}.

joints{2} link
with-parent-point (0.0, .53, 0)
with-child-point (0, -.305, 0)
to-child links{2}
parent links{0}.

joints{4} link
with-normal ( 1, 0, 0 )
with-parent-point (0, .305, 0)
with-child-point (0, -.33, 0)
to-child links{4}
parent links{2}.

joints{6} link
with-parent-point (0, .33, 0)
with-child-point (0, -.23, .35)
to-child links{6}
parent links{4}.

self set-root to bodyLink.

# rotate the creature and move it to above the origin.

self rotate around-axis (1, 0, 0) by 1.57.

joints set-double-spring with-strength 400 with-max .8 with-min -.8.
joints set-strength-limit to 300.

+ to center:
# to center the object, we set the X and Z components to 0, but not
# the Y, otherwise we would push the walker into the ground

currentLocation (vector).

currentLocation = (self get-location).
self move to (0, currentLocation::y, 0).

# The following four method allow external objects to manipulate
# the torque values of the links.

+ to set-joint-velocity-1 to value (vector):
joints{1} set-joint-velocity to value.

+ to set-joint-velocity-2 to value (vector):
joints{2} set-joint-velocity to value.

+ to set-joint-velocity-3 to value (float):
joints{3} set-joint-velocity to value.

+ to set-joint-velocity-4 to value (float):
joints{4} set-joint-velocity to value.

+ to set-joint-velocity-5 to value (vector):
joints{5} set-joint-velocity to value.

+ to set-joint-velocity-6 to value (vector):
joints{6} set-joint-velocity to value.

+ to destroy:
free links.
free bodyLink.
}

Attachment	Size
Picture 7.png	48.09 KB