@use PhysicalControl. @use Shape. @use Stationary. @use Link. @use MultiBody. @use NeuralNetwork3. @define BRAITENBERG_MAX_VELOCITY 3. @define DECAY 0.025. @define ACCEL 5. @define ARM_ACCEL 0.0001 . PhysicalControl : BraitenbergControl2 { % This class is used for building simple Braitenberg vehicle % simulations. To create a Braitenberg vehicle simulation, % subclass BraitenbergControl and use the init method to % create OBJECT(BraitenbergLight) and % OBJECT(BraitenbergVehicle) objects. + variables: floor (object). floorShape (object). cloudTexture (object). + to init: self enable-lighting. self enable-smooth-drawing. floorShape = new Shape. floorShape init-with-cube size (400, .2, 400). floor = new Stationary. floor register with-shape floorShape at-location (0, 0, 0). floor catch-shadows. self point-camera at (0, 0, 0) from (6, 6, 24). self enable-shadows. self enable-reflections. self enable-fast-physics. self set-fast-physics-iterations to 15. cloudTexture = (new Image load from "images/clouds.png"). self set-background-color to (.4, .6, .9). self set-background-texture-image to cloudTexture. #super enable-outline. #super init. } MultiBody : BraitenbergVehicle2 (aka BraitenbergVehicles2) { % This object is used in conjunction with OBJECT(BraitenbergControl) to % create simple Braitenberg vehicles. + variables: bodyShape (object). wheelShape (object). sensorShape (object). armShape(object). lightShape(object). bodyLink (object). buildPointers (list). brain (object). wheels (list). sensors (list). lights(list). arms(list). kind(int). kiddy(object). carried(object). carriedSize(vector). resize(vector). lift(int). peakIn(double). peakOut(double). #+ to destroy: #temp(object). #print"destroying braitenbergvehicle2". #free sensorShape. #free wheelShape. #free bodyShape. #free armShape. #free lightShape. #if kiddy: free kiddy. #if carried: free carried. #if bodyLink: free bodyLink. #if buildPointers: free buildPointers. #if brain: free brain. #foreach temp in wheels: free temp. #foreach temp in sensors: free temp. #foreach temp in lights: free temp. #foreach temp in arms: free temp. #print"destroying braitenbergvehicle2". #super destroy. #free super. #self free-all-connected-objects. #free self. + to init: super init. bodyShape = new Shape. bodyShape init-with-cube size (4.0, .75, 3.0). wheelShape = new Shape. wheelShape init-with-polygon-disk radius .6 sides 40 height .1. sensorShape = new Shape. sensorShape init-with-polygon-cone radius .2 sides 10 height .5. armShape = new Shape. armShape init-with-cube size (0.1, 1.0, 0.1). lightShape = new Shape. lightShape init-with-sphere radius 0.3. peakOut = 0. #self move to (0, 1.0, 0). self set-texture-scale to 1.5. super enable-self-collisions. #self setup-collision-handlers. self disable-auto-free. # brain management + to init-and-start-brain of size(int): #if brain: free brain. brain = new NeuralNetwork3. brain init-with neuron-count size. #brain init-random-structure with-connectivity-percent 100.0 with-autapses 0. brain init-fully-connected-structure with-autapses 1. #self add-dependency on brain. brain randomize-weights between -1.0. if wheels: wheels set-brain to brain. if arms: arms set-brain to brain. if sensors: sensors set-brain to brain. if lights: lights set-brain to brain. return brain. + to start-brain of b(object): nully(object). #carried = nully. #kiddy = nully. #print " Starting new brain of $b ". #if brain: free brain. brain = b. #brain re-states. #print " Starting new brain2 of $brain ". if wheels: wheels set-brain to brain. if sensors:{ sensors set-brain to brain. #print " in start brain of manipulator sensors are $sensors ". } #if lights: lights set-brain to brain. if arms: arms set-brain to brain. #self setup-collision-handlers. #free(nully). return self. + to set-brain of b(object): #if brain: free brain. brain = b. + to get-brain: return brain. + section "Adding Wheels, Lights, Arms, Sensors to a Vehicle" + to add-body with seg(object): bodyLink = seg. #new Segment. #bodyLink set-shape to bodyShape. bodyLink set-mu to -1.0. bodyLink set-eT to .8. bodyLink set-color-integer to 0. self set-root to bodyLink. buildPointers{0} = bodyLink. + to add-wheel at location (vector): % Adds a wheel at location on the vehicle. This method returns % the wheel which is created, a OBJECT(BraitenbergWheel). You'll % use the returned object to connect it to the vehicle's sensors. wheel, jnt (object). wheel = new BraitenbergWheel2. wheel set-shape to wheelShape. jnt = new RevoluteJoint. wheel set-joint to jnt. wheel rotate around-axis (1,0,0) by 1.57 . jnt set-relative-rotation around-axis (1, 0, 0) by 1.57. jnt link parent bodyLink to-child wheel with-normal (0, 0, 1) with-parent-point location with-child-point (0, 0, 0). wheel set-eT to .8. wheel set-texture to 0. wheel set-joint to jnt. jnt set-strength-limit to (jnt get-strength-hard-limit). #jnt enable-automatic-joint-scaling. wheel set-color to (.6, .6, .6). wheel set-mu to 100000. #self add-dependency on jnt. #self add-dependency on wheel. push wheel onto wheels. return wheel. + to add-sensor at location (vector) of-kind t (int) with-normal norm (vector): % Adds a sensor at location on the vehicle. This method returns % the sensor which is created, a OBJECT(BraitenbergSensor). You'll % use the returned object to connect it to the vehicle's wheels. sensor, joint (object). sensor = new BraitenbergSensor2. #print "add sensor of manipulator just build sensor $sensor". sensor add-kind of t. sensor set-shape to sensorShape. sensor rotate around-axis (0,0,1) by 1.57 . joint = new RevoluteJoint. joint set-relative-rotation around-axis (0, 0, 1) by -1.57. joint link parent bodyLink to-child sensor with-normal norm with-parent-point location with-child-point (0, 0, 0). joint set-double-spring with-strength 300 with-max 0.01 with-min -0.01. #joint enable-automatic-joint-scaling. #self add-dependency on joint. #self add-dependency on sensor. sensor set-color to (0, 0, 0). push sensor onto sensors. return sensor. + to add-light at location (vector) of-kind t (int): light, jnt(object). light = new BraitenbergLight2. light set-shape to lightShape. light set-color-integer to t. self set-kind to t. jnt = new RevoluteJoint. jnt link parent bodyLink to-child light with-normal (1, 0, 0) with-parent-point location with-child-point (0, 0, 0). #jnt enable-automatic-joint-scaling. #self add-dependency on jnt. #self add-dependency on light. push light onto lights. + to add-new-arm: buildPointers{|buildPointers|} = bodyLink. + to add-arm-segment of seg (object) to-limb limb(int) at-parent-location location (vector) with-child-location childPoint (vector) with-normal norm (vector) with-rotation rot(vector): % Adds a wheel at location on the vehicle. This method returns % the wheel which is created, a OBJECT(BraitenbergWheel). You'll % use the returned object to connect it to the vehicle's sensors. jnt (object). jnt = new RevoluteJoint. #seg set-joint to jnt. seg rotate around-axis (1,0,0) by 1.57 . jnt set-relative-rotation around-axis rot by 1.57. jnt link parent (buildPointers{limb}) to-child seg with-normal norm with-parent-point location with-child-point childPoint. seg set-joint to jnt. jnt set-strength-limit to (jnt get-strength-hard-limit). #jnt enable-automatic-joint-scaling. jnt set-joint-velocity to 5.0 . #self add-dependency on jnt. #self add-dependency on seg. buildPointers{limb} = seg. push seg onto arms. return seg. + to set-kind to t(int): kind = t. lights set-color-integer to t. + to set-all-kinds to t(int): tmp(object). kind = t. if bodyLink: bodyLink set-color-integer to t. if lights: lights set-color-integer to t. foreach tmp in arms: tmp set-color-integer to t. foreach tmp in wheels: tmp set-color-integer to t. foreach tmp in sensors: tmp set-color-integer to t. #free(tmp). + to get-kind: return kind. + to set-all-parent: tmp(object). if bodyLink: bodyLink set-parent of self. if lights: lights set-parent of self. foreach tmp in arms: tmp set-parent of self. foreach tmp in wheels: tmp set-parent of self. foreach tmp in sensors: tmp set-parent of self. + to integrate-outboard-sensor obj sensr (object): push sensr onto sensors. + to control at-time t(float): tamp(object). vals(list). val(double). z(int). start(int). slide(int). if (brain && sensors):{ foreach tamp in sensors:{ #print "sensor in control of manipulator is $tamp". #print "can sensor respond to? $tamp". if (tamp can-respond to "get-strengths"):{ #print "tmp can respond to get-stregths". } #tmp iterate. #(tmp get-strengths). vals = tamp get-strengths. #vals{0}=0. #vals{0}=1. #vals{1}=1. #vals{2}=1. #vals{3}=1. start = (tamp get-start). for z = 0, z < |vals| , z++:{ val = (vals{z}) * 100. slide = start + z. if start:{ #print "$slide XXXXXXXXXXXXXXXX $val". brain set-neuron-state at slide to val. } } } } slide += 1. brain update-state. #slide = |sensors|. if carried:{ val = 1. brain set-neuron-state at slide++ to val. carriedSize = (carried get-size). val = (carriedSize::x). brain set-neuron-state at slide++ to val. val = (carriedSize::y). brain set-neuron-state at slide++ to val. val = (carriedSize::z). brain set-neuron-state at slide to val. } else { val = 0. brain set-neuron-state at slide to val. } #brain update-state. foreach tamp in arms:{ start = (tamp get-start). slide = start. #reuse in other direction val = (brain get-neuron-state at start). val = val * ARM_ACCEL. tamp activate with-input val. if (|val| > peakOut): peakOut = |val|. #print "$start outputarmX $val". } foreach tamp in wheels:{ start = (tamp get-start). val = (brain get-neuron-state at start). val = val * ACCEL. tamp activate with-input val. if (|val| > peakOut): peakOut = |val|. #print "$start outputwheelX $val". } if ((brain get-neuron-state at (slide - 1)) > 0):{ #try --slide lift = 1. self magnet-to-nearest. resize = ((brain get-neuron-state at (slide - 2)), (brain get-neuron-state at (slide - 3)), (brain get-neuron-state at (slide - 1))). } else:{ lift = 1.#xxxxxxxxxxxxxxxxxxxxxxxxxredoxxxxxxxxxxxxxxxxxxxxxx resize = (1,1,1). } #brain update-state. brain decay-by ratio DECAY. #brain decay-meta-by ratio DECAY. #print "absolute value of peak output $peakOut". peakOut = 0. #brain re-states. #} #free(tamp). #super iterate. + to magnet-to-nearest: + to center: currentLocation (vector). currentLocation = (self get-location). self move to ( 1 , (currentLocation::y), 1). + to set-child to kid(object): kiddy = kid. + to get-child: #kiddy child. return kiddy. + to set-carried to car(object): carried = car. + to get-carried: return carried. + to get-arms: return arms. + to get-wheels: return wheels. + to get-sensors: return sensors. + to get-body: return bodyLink. + to get-buildPointer for-limb limb(int): return (buildPointers{limb}). + to get-lift: return lift. + to get-fitness: fitness(double). armlist(list). tmp(object). otmp(object). otmptype(string). i(int). dif(double). near(double). otmpvect(vector). # fitness = 0. near = 1000. if kiddy:{ fitness += 1000. #bonus armlist = (kiddy get-arms). tmp = (kiddy get-body). dif = |(tmp get-size) - (bodyLink get-size)|. fitness -= dif. for i=0, i < |armlist|, i++:{ fitness += 333. if (arms{i}):{ dif = |((arms{i}) get-size) - ((armlist{i}) get-size)|. fitness -= dif. } } } else if carried:{ fitness += 1000. } else { foreach tmp in (all Segment):{ if ((tmp get-color-integer) == 1):{ otmp = (buildPointers{|buildPointers| - 1}). #print "other temp $otmp". otmpvect = (otmp get-location). dif = |otmpvect - (tmp get-location)|. #print "diff $dif". if dif < near:{ near = dif. #print "min diff $near". } } } fitness += (250 - near). #print "min diff $near". } return fitness. } Link : Segment (aka Segments) { + variables: cap(object). #the cap is what is connected to the tjoint when it isn't carrying tipjoint(object). #tipjoint is the joint for the cap joint(object). hull(object). brain(object). #pointer to brain start(int). #start of sensors input space or start of output vector space on brain last(int). #signify that it is the last segment to use the brain therefor should reset states color(int). size(vector). startPosition(vector). parent(object). #+ to destroy: #if hull: free hull. #if joint: free joint. #if brain: free brain. #if tipjoint: free tipjoint. #if cap: free cap. #super destroy. #self free-all-connected-objects. #free self. + to init: # super init. self disable-auto-free. self setup-collision-handlers. + to return-to-start: self move to startPosition. + to set-start-position: startPosition = self get-location. + to set-brain to b(object): #if brain: free brain. #print "$self is setting brain to $b". brain = b. + to set-parent of rent(object): parent = rent. + to get-parent: return parent. + to set-start to d(int): start = d. + to get-start: return start. + to get-size: return size. + to get-shape: return hull. + to set-shape to s(object) of sz(vector): hull = s. size = sz. super set-shape to s. + to scale by scale (vector): size = size * scale. hull scale by scale. + to set-joint to j(object): joint = j. + to set-last: last = 1. + to get-point-on-shape on-vector theVector (vector): #This method is experimental. #Starting from inside the shape at the center, this function goes in the direction of theVector until it hits the edge of the shape. The resulting point is returned. #This allows you to compute link points for arbitrary shapes. For example, if you want to compute a link point for the "left-most" point on the shape, you can call this method with (-1, 0, 0). #Returns (0, 0, 0) if the shape is not initialized or if an error occurs. return (hull get-point-on-shape on-vector theVector). + to set-color-integer to col(int): color = col. if col == 0: self set-color to (0,1,0). #trial vehicle if col == 1: self set-color to (1,0,0). # segments if col == 2: self set-color to (0,0,1). #carried if col == 3: self set-color to (0,0,0). #child if col == 4: self set-color to (1,1,1). # sensors if col == 5: self set-color to (.3, .3, .3). # wheels if col == 7: self set-color to (.6, .6, .6). # lights XXXX Note: create light signals as output + to get-color-integer: return color. + to add-tip at location (vector) of-kind t (int) with-normal norm (vector): lil(object). lil = (new Cube init-with size (.05,.05,.05)).#(.01, .01, .01)). cap = new Link. cap set-shape to lil. #cap draw-as-point. tipjoint = new RevoluteJoint. tipjoint link parent self to-child cap with-normal norm with-parent-point location with-child-point (0, 0, 0). tipjoint enable-automatic-joint-scaling. + to get-tip-loc: return (cap get-location). + to get-tipjoint: return tipjoint. + to lift a seg(object) at location (vector) with-normal norm(vector) and child-loc(vector): tipjoint link parent self to-child seg with-normal norm with-parent-point location with-child-point child-loc. + to add-sensor at location (vector) of-kind t (int) with-normal norm (vector): % Adds a sensor at location on the vehicle. This method returns % the sensor which is created, a OBJECT(BraitenbergSensor). You'll % use the returned object to connect it to the vehicle's wheels. sensor, jnt (object). sensorShape (object). sensorShape = new Shape. sensorShape init-with-polygon-cone radius .05 sides 10 height .12. sensor = new BraitenbergSensor2. sensor add-kind of t. sensor set-shape to sensorShape. #sensor rotate around-axis (1,0,0) by .7534. #1.57. jnt = new RevoluteJoint. jnt set-relative-rotation around-axis (0, 1, 0) by -1.57. jnt link parent self to-child sensor with-normal norm with-parent-point location with-child-point (0, 0, 0). jnt set-double-spring with-strength 300 with-max 0.01 with-min -0.01. #jnt enable-automatic-joint-scaling. jnt set-joint-velocity to 5.0 . #self add-dependency on jnt. #self add-dependency on sensor. sensor set-color to (0, 0, 0). return sensor. + to setup-collision-handlers: self handle-collisions with-type "Segment" with-method "affix". # Mobile: get-colliding-objects # + to affix to seg(object): mul(object). othercolor(int). tmp(object). car(object). jnt(object). shp(object). child(object). spot(vector). ospot(vector). tester(string). lft(int). knd(int). print "affix of Segment $self to $seg". #tester = ((super get-multibody) get-kind). #print "tester $tester". #tester = ((super get-multibody) get-type). #print "tester2 $tester". if (self get-parent): mul = (self get-parent). if mul:{ print "this one $self has parent $mul". car = (mul get-carried). } othercolor = 0. if !car && mul :{ othercolor = seg get-color-integer. lft = (mul get-lift). knd = ((mul get-kind) == 0). print "this one $self is touching $seg with color $othercolor while lift is $lft and multibody knd is $knd". # && (self == (mul get-buildPointer for-limb 0)) if ((othercolor == 1) && lft && knd):{ print " inside the lift create condition ". if !(mul get-child) :{ child = new BraitenbergVehicle2. tmp = (brain get-copy). child set-brain of tmp. child add-body with seg. child set-all-kinds to 2. mul set-child to child. print "creating childXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx". } else { #jnt = self get-tipjoint. tmp = (mul get-child).#was jnt's child spot = (tmp get-location) - (seg get-location). # Mobile: transform world-vector theVector (vector) shp = (seg get-shape). spot = shp get-point-on-shape on-vector spot. ospot = (jnt get-parent-link-point). jnt break. self lift a seg at ospot with-normal (0,0,0) and spot. #free tmp. mul set-carried to seg. print "lifting XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXx". } } if ((othercolor == 1) && (color == 3)):{ tmp = (mul get-buildPointer for-limb 0). ospot = (tmp get-tip-loc). spot = ospot - (seg get-location). shp = (seg get-shape). spot = shp get-point-on-shape on-vector spot. self add-arm-segment of seg at-parent-location ospot with-child-location spot with-normal (0, 1, 0) with-rotation (0, 1, 0). seg set-brain to brain. } } } Segment : BraitenbergLight2 (aka BraitenbergLights2) { % A BraitenbergLight is used in conjunction with OBJECT(BraitenbergControl) % and OBJECT(BraitenbergVehicle). It is what the OBJECT(BraitenbergSensor) % objects on the BraitenbergVehicle detect. %
% There are no special behaviors associated with the lights--they're % basically just plain OBJECT(Mobile) objects. #Note: use light signals as output from host manipulator to neighbors + to init: #super init. self set-shape to (new Shape init-with-sphere radius .3). super set-color-integer to 0. # Until we change it's flavor. self disable-auto-free. } #location = (self get-location). #location::z = 0. #self move to location. Segment : BraitenbergWheel2 (aka BraitenbergWheels2) { % A BraitenbergWheel is used in conjunction with OBJECT(BraitenbergVehicle) % to build Braitenberg vehicles. This class is typically not instantiated % manually, since OBJECT(BraitenbergVehicle) creates one for you when you % add a wheel to the vehicle. %
% NOTE: this class is included as part of the file "Braitenberg.tz". + variables: #joint (object). naturalVelocity (float). newVelocity, oldVelocity (float). + to init: super init. naturalVelocity = 0. newVelocity = 0. self disable-auto-free. #+ to destroy: #free joint. #super destroy. #self free-all-connected-objects. #free self. + section "Configuring the Wheel's Natural Velocity" + to set-natural-velocity to n (float): % Sets the "natural" velocity of this wheel. The natural velocity % is the speed at which the wheel turns in the absence of sensor % input. naturalVelocity = n. + to activate with-input n (float): % Used internally. newVelocity = n. + to reset-velocity: joint set-joint-velocity to 0. + to iterate: #if brain: newVelocity = (brain get-neuron-state at start). if newVelocity > BRAITENBERG_MAX_VELOCITY: newVelocity = BRAITENBERG_MAX_VELOCITY. if newVelocity < -BRAITENBERG_MAX_VELOCITY: newVelocity = -BRAITENBERG_MAX_VELOCITY. # this oldVelocity stuff is used to limit the deceleration so that if newVelocity == 0: { joint set-joint-velocity to oldVelocity. oldVelocity *= .95. } else { joint set-joint-velocity to newVelocity. oldVelocity = newVelocity. } newVelocity = naturalVelocity. #if last: brain re-states. } Segment : BraitenbergSensor2 (aka BraitenbergSensors2) { % A BraitenbergSensor is used in conjunction with OBJECT(BraitenbergVehicle) % to build Braitenberg vehicles. This class is typically not instantiated % manually, since OBJECT(BraitenbergVehicle) creates one for you when you % add a sensor to the vehicle. %
% NOTE: this class is included as part of the file "Braitenberg.tz". + variables: wheels (list). bias (float). direction (vector). sensorAngle (float). activationObject (object). activationMethod (string). kinds(list). strengths(list). cnt(int). #+ to destroy: #if activationObject: free activationObject. #super destroy. #self free-all-connected-objects. #free self. + to init: super init. bias = 1.0. direction = (0, 1, 0). sensorAngle = 1.0. self disable-auto-free. + section "Linking the Sensor to a Wheel" + to link to w (object): % Associates this sensor with wheel w. push w onto wheels. + section "Configuring the Sensor Values" + to add-kind of d (int): #print " adding kind of $d to $self ". push d onto kinds. push 1 onto strengths. #print " added to kind list $kinds and $strengths ". + to get-kinds: return kinds. + to get-strengths: temporary(list). temporary{0} = 0.0. #print " getting-strengths in $self of $strengths". if (strengths):{ return strengths. } else:{ #print "returning tempMMMMMMMMMMMMMMMM". return temporary. } + to re-states: w(int). if (strengths):{ for w = 0, w < |strengths|, w++:{ strengths{w} = 0.0 . } } + to set-direction to d(vector): direction = d. + to set-bias to d (float): % Sets the "bias" of this sensor. The default bias is 1, meaning % that the sensor has a positive influence on associated wheels % with strength 1. You can change this to any magnitude, positive % or negative. bias = d. + to set-sensor-angle to n (float): % Sets the angle in which this sensor can detect light. The default % value of 1.5 means that the sensor can see most of everything in % front of it. Setting the value to be any higher leads to general % wackiness, so I don't suggest it. sensorAngle = n. + to set-activation-method to m (string) in-object o (object): % This method specifies an activation method for the sensor. An % activation method is a method which takes as input the strength % read by the sensor, and as output returns the strength of the % signal which will travel on to the motor. %
% Your activation function should be defined as: %
% + to activation-function-name with-sensor-strength s (float): %%
% The default activation method is linear, but more complex vehicles % may require non-linear activation functions. % activationMethod = m. activationObject = o. + to iterate: i (object). lights (int). total, strength, angle (float). toLight, transDir (vector). n(int). #print " iterating sensor strengths $strengths". transDir = (self get-rotation) * direction. for n=0, n<|kinds|, n++:{ strengths{n}= 0.0. #print "cycling through kinds setting strengths $n". } for n=0, n<|kinds|, n++:{ foreach i in (all Segments): { if ((i get-color-integer) == (kinds{n}) ):{ toLight = (i get-location) - (self get-location). angle = angle(toLight, transDir). if angle < sensorAngle: { strength = | (self get-location) - (i get-location) |. strength = 1.0 / (strength * strength). if activationMethod && activationObject: { strength = (activationObject call-method named activationMethod with-arguments { strength }). } if strength > 10: strength = 10. total += strength. lights++. } } if lights != 0: total /= lights. total = 50 * total * bias. strengths{n} = strengths{n} + total. #print "activation of sensors of type $n to val $total". total = 0. } } #if lights != 0: total /= lights. #total = 50 * total * bias. #print " strengths in iterate $strengths ". #for n = start, n < (|strengths| + start), n++:{ #if brain: brain set-neuron-state at n to strengths{n - start}. #strengths{n - start} = 0. #} }