class Test_Setup(object):
""" Represent the components of the simulation. """
def __init__(self, man):
""" Initialize the simulation objects. """
self.man = man
# Sensors for robot.
self.sensor = Sensors(man)
# Create the main box.
self.__create_main_box()
# Create the sphere.
self.__create_sphere()
def __create_main_box(self):
""" Create the main box and attach it by a rotating hinge to the ground. """
self.man.create_box(0, [1., 1., 1.], [0., 0.5, 0.], density=5.0)
self.man.create_ray(1, [0.5, 0.5, 0.0], [0, -90, 0])
self.man.create_hinge(0, [0.5, 0.5, 0.], [0, 1],
axis=[0., 1., 0.],
lims=[-1., 1.],
max_force=100.)
# Create the distance sensor.
self.sensor.add_dist_sensor(self.man.geoms[1], 5.0)
def __create_sphere(self):
""" Create a sphere """
self.man.create_sphere(3, 4.0, 0.5, [3.0, 0.5, 0.0])
self.man.create_sphere(4, 4.0, 0.5, [4.0, 0.5, 0.0])
def get_ray_distance(self):
return self.sensor.get_raw_distance(self.man.geoms[1])
def set_ray_distance(self, dist):
self.sensor.set_distance(self.man.geoms[1], dist)
def reset_ray(self):
self.sensor.reset_distance_sensor(self.man.geoms[1])
class Worm(object):
def __init__(self,
man,
morphology_genome={},
base_pos=[0, 0, 0],
num_joints=2,
logging=False,
log_path='./',
logfileprefix=''):
""" Initialize the robot in the ODE environment.
Arguments:
man: ODE Manager for the Physics Simulation
base_pos: base position to start the robot from
morphology_genome: dict of dicts which contain different parameters for the morphology (TODO)
num_joints: number of joints in the worm robot
"""
self.man = man
self.body_keys = []
self.logging = logging
self.logfileprefix = logfileprefix
# Sensors for robot.
self.sensor = Sensors(man=man, logging=logging, log_path=log_path)
self._num_joints = num_joints
# morphology of the robot
# first element in the list is the
self.freq = morphology_genome[0]
assert self.freq >= 0, "frequency shall not be negative!"
# Initialize the robot.
self.__create_robot(base_pos=base_pos)
def __create_robot(self, base_pos=[0, 0, 0]):
""" Create the robot used in the experiment.
Arguments:
base_pos: base position to start the robot from
morphology: optional dict of dicts defining measurements for various parts of the robot.
"""
# Constants for the different body parts.
# Main Body
WORM_LENGTH = 10.
#SEG_DIMS = morphology['seg_dims'] if 'seg_dims' in morphology else [WORM_LENGTH/(self._num_joints+1),.50,.50]
# calculate the dimensions of a segment
SEG_DIMS = [
WORM_LENGTH / (self._num_joints + 1) # x -length
,
.50 # y -length
,
.50
] # z -length
#BODY_POS = [morphology['body_pos'][0]-base_pos[0],morphology['body_pos'][1]-base_pos[1],morphology['body_pos'][2]-base_pos[2]] \
# if 'body_pos' in morphology else [((SEG_DIMS[0]*self._num_joints)/2.)+base_pos[0],1+base_pos[1],0.0+base_pos[2]]
# calculate the starting point of the body in the simulation environment
BODY_POS = [
((SEG_DIMS[0] * self._num_joints) / 2.) + base_pos[
0] # align the body in x - direction, middle of the body is at base position
,
1 + base_pos[1] # lift the body in +y - direction by 1
,
0.0 + base_pos[2]
] # set the middle of the body's z starting position to the base position
#BODY_MASS = morphology['body_mass'] if 'body_mass' in morphology else 5.
BODY_MASS = 50 # hardcoded body mass value, that worked in jared's simulations
# joint maximum forces, todo think about the values
JOINT_FORCE = [100., 100.]
joint_range = math.radians(90.)
# Keep track of body and joint numbers.
b_num = 0
j_num = 0
# Create the Segments
self.body_keys.append(
self.man.create_box(b_num,
SEG_DIMS,
BODY_POS,
density=BODY_MASS / (self._num_joints + 1)))
BODY_POS[0] -= SEG_DIMS[0]
b_num += 1
for s in range(self._num_joints):
self.body_keys.append(
self.man.create_box(b_num,
SEG_DIMS,
BODY_POS,
density=BODY_MASS /
(self._num_joints + 1)))
con_point = [
BODY_POS[0] + SEG_DIMS[0] / 2., BODY_POS[1], BODY_POS[2]
]
self.man.create_universal(j_num,
con_point, [b_num - 1, b_num],
axis1=[0, 0, -1],
axis2=[0, 1, 0],
loStop1=-joint_range,
hiStop1=joint_range,
loStop2=-joint_range,
hiStop2=joint_range,
fmax=JOINT_FORCE[0],
fmax2=JOINT_FORCE[1])
BODY_POS[0] -= SEG_DIMS[0]
b_num += 1
j_num += 1
print(b_num, j_num)
# Add in joint positions sensors.
self.sensor.register_joint_sensors([i for i in range(j_num)])
# Add distance sensors to the head.
# taken from Jared's code
# Reset body position so we put sensors on the head.
BODY_POS = [((SEG_DIMS[0] * self._num_joints) / 2.) + base_pos[0],
1 + base_pos[1], 0.0 + base_pos[2]]
# Forward looking sensor
sensor_pos = [BODY_POS[0] + SEG_DIMS[0] / 2., BODY_POS[1], BODY_POS[2]]
self.man.create_ray(b_num, sensor_pos, [0, -90, 0])
self.man.create_hinge(j_num,
sensor_pos, [0, b_num],
axis=[0., 1., 0.],
lims=[0., 0.],
max_force=100.)
# Register the sensor.
self.sensor.add_dist_sensor(self.man.geoms[b_num], 5.0)
b_num += 1
j_num += 1
# Right looking sensor
sensor_pos = [
BODY_POS[0] + SEG_DIMS[2] / 2., BODY_POS[1],
BODY_POS[2] + SEG_DIMS[2] / 2.
]
self.man.create_ray(b_num, sensor_pos, [0, 0, 0])
self.man.create_hinge(j_num,
sensor_pos, [0, b_num],
axis=[0., 1., 0.],
lims=[0., 0.],
max_force=100.)
# Register the sensor.
self.sensor.add_dist_sensor(self.man.geoms[b_num], 15.0)
b_num += 1
j_num += 1
# Left looking sensor
sensor_pos = [
BODY_POS[0] + SEG_DIMS[2] / 2., BODY_POS[1],
BODY_POS[2] - SEG_DIMS[2] / 2.
]
self.man.create_ray(b_num, sensor_pos, [0, 90, 0])
self.man.create_hinge(j_num,
sensor_pos, [0, b_num],
axis=[0., 1., 0.],
lims=[0., 0.],
max_force=100.)
# Register the sensor.
self.sensor.add_dist_sensor(self.man.geoms[b_num], 15.0)
b_num += 1
j_num += 1
def get_sensor_states(self):
""" Get the states of the various sensors on the robot. """
sensors = [i for i in self.sensor.get_joint_sensors()]
sensors = [sensors[i] for i in range(0, len(sensors), 2)
] # Remove sensors for vertical axis movement (Irrelevant).
sensors += [i for i in self.get_dist_sensor_data()
] # Add distance sensor data.s
return sensors
def actuate_joints_by_pos(self, positions):
""" Actuate the joints of the worm to the specified position.
Arguments:
positions: position of the joints to actuate to.
"""
# actuate all joint according to the expected position
for i, p in enumerate(positions):
self.man.actuate_universal(
i, 0, p
) # joint1 = 0, because we use only one joint here -> movement in z axis
def get_avg_position(self):
""" Get the average position of the robot. """
avg_pos = [0, 0, 0]
for i in range(self._num_joints + 1):
avg_pos = [
sum(x) for x in zip(avg_pos, self.man.get_body_position(i))
]
# Average the positions.
avg_pos = [i / (self._num_joints + 1) for i in avg_pos]
return avg_pos
def clear_sensors(self):
""" Clears all sensor values
"""
self.sensor.clear_sensors()
def log_sensor_data(self):
self.sensor.dump_sensor_data(self.logfileprefix)
def sensor_step(self, cur_time):
self.sensor.step(cur_time)
def reset_touch_sensors(self):
""" Reset the touch sensors. """
self.sensor.clear_touching()
def get_ray_distance(self, geom):
return self.sensor.get_raw_distance(
self.man.geoms[self.man.get_geom_key(geom)])
def get_scaled_ray_distance(self, geom):
return self.sensor.get_scaled_distance(
self.man.geoms[self.man.get_geom_key(geom)])
def get_dist_sensor_data(self):
return self.sensor.get_scaled_distances()
def set_ray_distance(self, geom, dist):
self.sensor.set_distance(self.man.geoms[self.man.get_geom_key(geom)],
dist)
def reset_distance_sensors(self):
self.sensor.reset_distance_sensors()