Commit 1bb1501e authored by David A.. Werner's avatar David A.. Werner
Browse files

Implemented simulate. Added ManducaBodyProperties. Implemended distance...

Implemented simulate. Added ManducaBodyProperties. Implemended distance traveled fitness function and added support for custom fitness functions.
parent e79df061
import types
import numpy as np
from scipy.integrate import odeint
from collections import namedtuple
import StringIO
import functools
class ManducaDeformityError(Exception):
def __init__(self, message):
self.message = message
......@@ -10,15 +12,39 @@ class ManducaDeformityError(Exception):
def __str__(self):
return repr(self.message)
# L0: Body segment resting spring lenght
# k: Elastic spring constant
# m: Leg mass
# c: Viscosity constant
ManducaBodyProperties = namedtuple('ManducaBodyProperties', ['L0', 'k', 'c', 'm'])
class Manduca(object):
def __init__(self, legs, muscles, time_step):
default_body_properties = ManducaBodyProperties(500, 1, 2, 1)
damped_body_properties = ManducaBodyProperties(500, 1, 3, 1)
def __init__(self, legs, muscles, time_step, fitness_function=None, body_properties=None):
self._legs = self._muscles = None
self.legs = np.copy(legs)
self.muscles = np.copy(muscles)
self._time_step = time_step
self._fitness = None
self.check_consistancy()
if fitness_function == None:
self._fitness_function = self.distance_traveled
else:
self._fitness_function = fitness_function
if body_properties == None:
self.body_properties = Manduca.default_body_properties
else:
self.body_properties = body_properties
@property
def fitness_function(self):
return self._fitness_function
@fitness_function.setter
def fitness_function(self, fn):
self._fitness = None
self._fitness_function = fn
@property
def legs(self):
......@@ -76,16 +102,27 @@ class Manduca(object):
raise(ManducaDeformityError(e))
return leg_ts
def distance_traveled(self):
positions, velocites, times = self.simulate(record_level=2)
mean_starting_loc = np.mean(positions[0])
mean_end_loc = np.mean(positions[-1])
distance_traveled = mean_end_loc - mean_starting_loc
return distance_traveled
@property
def fitness(self):
if (self._fitness is None):
results = self.simulate()
self._fitness = 0
if isinstance(self._fitness_function, types.MethodType) or \
isinstance(self._fitness_function, types.UnboundMethodType):
self._fitness = self._fitness_function()
elif isinstance(self._fitness_function, types.LambdaType) or \
isinstance(self._fitness_function, types.FunctionType):
self._fitness = self._fitness_function(self)
return (self._fitness)
@property
def initial_position(self):
return np.arange(self.num_legs)*500
return np.arange(self.num_legs)*self.body_properties.L0
def check_consistancy(self):
if self.num_muscles != self.num_legs - 1:
......@@ -122,8 +159,118 @@ class Manduca(object):
manduca_str = leg_str + m_str + t_str
return hash(manduca_str)
def simulate(self):
pass
def simulate(self, record_level=0, verbose_record=True, num_sub_intervals = 10):
""" record_level: Which intervals to record the position/velocity/time
0 -> initial/final only
1 -> once for each interval
2 -> every sub-interval
* -> default to 0
"""
L0, k, c, m = self.body_properties
def calculate_slopes(x, t):
"""This function computes the derivatives of x
x is position of legs and velocites of legs
in the form [x0, x1, ..., xn, v0, v1, ..., vn]
t is an array of time values over which to integrate
in the form [t0, t1, ..., tN]
The forces are as follows:
Back leg: f0' = k(x1-x0-L0) + c(v1-v0) + M01
Middle legs: fi' = k(x{i+1}-xi-L0) - k(xi-x{i-1}-L0) +
c(v{i+1}-vi) + c(v{i-1}-vi) + Mn{n+1} - M{n-1}n
Front Leg: fn' = -k(xn-x{n-1}-L0) + c(v{n-1}-vn) + M{n-1}n
"""
# Pull out the position and velocity vectors
pos, vel = x[0:self.num_legs], x[self.num_legs:2*self.num_legs]
# Manually set the velocity of locked legs to zero
vel[np.where(current_legs==1)] = 0
# The derivative of position is just velocity
d_pos = vel
# An empty array for the derivative of velocity: acceleration
d_vel = np.zeros(self.num_legs)
# Compute the acceleration for each leg
for curr_idx in range(self.num_legs):
prev_idx, next_idx = curr_idx - 1, curr_idx + 1
# Compute the forward forces
if next_idx < self.num_legs: #all but the last leg
M_forwards = current_muscles[curr_idx]
spring_forwards = k*(pos[next_idx] - pos[curr_idx] - L0)
damp_forwards = c*(vel[next_idx] - vel[curr_idx])
else: # This is the last leg; no forward forces
M_forwards = spring_forwards = damp_forwards = 0
# Compute the backward forces
if prev_idx > 0: # all but the first leg
M_backwards = current_muscles[prev_idx]
spring_backwards = k*(pos[curr_idx] - pos[prev_idx] - L0)
damp_backwards = c*(vel[curr_idx] - vel[prev_idx])
else: # This is the first leg; no backward forces
M_backwards = spring_backwards = damp_backwards = 0
# Compute the combined forces
M_force = M_forwards - M_backwards
spring_force = spring_forwards - spring_backwards
damp_force = damp_forwards - damp_backwards
acceleration = (M_force + spring_force + damp_force) / m
d_vel[curr_idx] = acceleration
# May not be necessary, but set the acceleration of locked legs to 0
d_vel[np.where(current_legs==1)] = 0
derivatives = np.hstack((d_pos, d_vel))
return (derivatives)
# Compute the time steps for each of the simulation intervals
end_time = self.num_time_steps * self.time_step
times = np.linspace(0, end_time, self.num_time_steps + 1)
# Initialize the position and velocity of each leg
position = self.initial_position
velocities = np.zeros((self.num_legs))
# Record the initial position and velocities
recorded_positions, recorded_velocities = [position], [velocities]
recorded_times = [0.0]
# For each time-step, advance the simulation
for time, current_legs, current_muscles in zip(times, self.legs, self.muscles):
end_time = time + self.time_step
# Split the interval up into the desired number of sub-intervals
interval_times = np.linspace(time, end_time, num_sub_intervals+1)
# Concatenate x and v to send to compute slopes
loc_and_vel = np.hstack ((position, velocities))
ode_values = odeint(calculate_slopes, loc_and_vel, interval_times)
# Extract the positions and velocities for all sub-intervals
# Exclude the initial value: it is a duplicate from the end of last interval!
new_positions = ode_values[1:, 0:self.num_legs]
new_velocities = ode_values[1:, self.num_legs:2*self.num_legs]
new_times = interval_times[1:]
# The current position and velocity are the last values in the list
position = new_positions[-1]
velocities = new_velocities[-1]
# Record the positions if desired
if record_level == 1:
recorded_positions.append(position)
recorded_velocities.append(velocities)
recorded_times.append(end_time)
elif record_level == 2:
recorded_positions.extend(new_positions)
recorded_velocities.extend(new_velocities)
recorded_times.extend(new_times)
return recorded_positions, recorded_velocities, recorded_times
def save(self, file_name, compressed=True):
if compressed:
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment