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)])
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()]
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)
class Worm(object):
def __init__(self,man,morphology_genome={},base_pos=[0,0,0],num_joints=2, morphology={}, 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
self.WORM_LENGTH = 0
self.BODY_MASS = 0
self.SEG_DIMS = [0,0,0]
self.JOINT_FORCE = [0.,0.]
# Initialize the robot.
self.__create_robot(base_pos=base_pos,morphology=morphology)
def __create_robot(self,base_pos=[0,0,0], morphology={}):
""" 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
self.WORM_LENGTH = 10.
#SEG_DIMS = morphology['seg_dims'] if 'seg_dims' in morphology else [self.WORM_LENGTH/(self._num_joints+1),.50,.50]
# calculate the dimensions of a segment
self.SEG_DIMS = [self.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 = [((self.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.
#self.BODY_MASS = 50 # hardcoded body mass value, that worked in jared's simulations
self.BODY_MASS = morphology['density'] if 'density' in morphology else 50. # TODO rename BODY_MASS to density
# joint maximum forces, todo think about the values
self.JOINT_FORCE = [10000.,10000.]
joint_range = math.radians(90.)
# Keep track of body and joint numbers.
b_num = 0
j_num = 0
# Create the Segments and define that inner surfaces are not used for drag forces calculation.
# These are all inner x surfaces of the boxes in this case.
# use the outer x surface of the robot for drag
self.body_keys.append(self.man.create_box(b_num,self.SEG_DIMS,BODY_POS,density=self.BODY_MASS/(self._num_joints+1)
, active_surfaces={'x':1,'y':1,'z':1,'-x':0,'-y':1,'-z':1}))
BODY_POS[0] -= self.SEG_DIMS[0]
b_num += 1
for s in range(self._num_joints):
if s == self._num_joints -1:
# use the outer x surface of the robot for drag
self.body_keys.append(self.man.create_box(b_num,self.SEG_DIMS,BODY_POS,density=self.BODY_MASS/(self._num_joints+1)
, active_surfaces={'x':0,'y':1,'z':1,'-x':1,'-y':1,'-z':1}))
else:
# don't use inner x surfaces for robot drag
self.body_keys.append(self.man.create_box(b_num,self.SEG_DIMS,BODY_POS,density=self.BODY_MASS/(self._num_joints+1)
, active_surfaces={'x':0,'y':1,'z':1,'-x':0,'-y':1,'-z':1}))
con_point = [BODY_POS[0]+self.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=self.JOINT_FORCE[0],fmax2=self.JOINT_FORCE[1])
BODY_POS[0] -= self.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)])
# Turn feedback on for the joints in order access the force and torque values
for i in range(self._num_joints):
self.man.joints[i].setFeedback()
def get_joint_feedback(self):
""" Get the feedback from the joints.
Per joint: force-body1 (x,y,z); torque-body1 (x,y,z); frce-body2 (x,y,z); torque-body2 (x,y,z)"""
joint_feedback = []
for i in range(self._num_joints):
joint_feedback.append(self.man.joints[i].getFeedback())
return joint_feedback
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()]
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 get_joint_pos(self, jointidx):
assert (jointidx >= 0 and jointidx <= self._num_joints), "Invalid joint index: %r" % jointidx
return self.man.get_body_position(jointidx)
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 log_avg_bodyposition(self,timestamp):
avgbodypos = self.get_avg_position()
with open("avg_bodypos.out", "a") as myfile:
myfile.write(`timestamp` + ' ' + `avgbodypos` + '\n')
def log_bodysegmentcenter(self,timestamp,bodyidx):
bodypos = self.man.get_body_position(bodyidx)
with open("body_" + `bodyidx`+ "_pos.out", "a") as myfile:
myfile.write(`timestamp` + ' ' + `bodypos` + '\n')
class Worm(object):
def __init__(self, man, logging=False, log_path='./', logfileprefix=''):
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 = 1
# Initialize the robot.
self.__create_robot()
def __create_robot(self):
# small segment
SEG_DIMS = [
1.50 # x -length
,
.50 # y -length
,
.50
] # z -length
BODY_POS = [
+SEG_DIMS[0] / 2 # x-pos
,
1. # y -pos
,
0.
] # z -pos
BODY_DENSITY = 50
MAXJOINT_FORCE = [10000., 10000.]
joint_range = math.radians(90.)
b_num = 0
self.body_keys.append(
self.man.create_box(key=b_num,
dim=SEG_DIMS,
pos=BODY_POS,
density=BODY_DENSITY,
active_surfaces={
'x': 0,
'y': 1,
'z': 1,
'-x': 1,
'-y': 1,
'-z': 1
}))
BODY_POS[0] += SEG_DIMS[0]
j_num = 0
for idx in range(1, 9):
b_num += 1
self.body_keys.append(
self.man.create_box(key=b_num,
dim=SEG_DIMS,
pos=BODY_POS,
density=BODY_DENSITY,
active_surfaces={
'x': 0,
'y': 1,
'z': 1,
'-x': 0,
'-y': 1,
'-z': 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=MAXJOINT_FORCE[0],
fmax2=MAXJOINT_FORCE[1])
j_num += 1
BODY_POS[0] += SEG_DIMS[0]
b_num += 1
self.body_keys.append(
self.man.create_box(key=b_num,
dim=SEG_DIMS,
pos=BODY_POS,
density=BODY_DENSITY,
active_surfaces={
'x': 1,
'y': 1,
'z': 1,
'-x': 0,
'-y': 1,
'-z': 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=MAXJOINT_FORCE[0],
fmax2=MAXJOINT_FORCE[1])
j_num += 1
BODY_POS[0] += SEG_DIMS[0]
self.sensor.register_joint_sensors([i for i in range(j_num)])
# Turn feedback on for the joints in order access the force and torque values
for i in range(j_num):
self.man.joints[i].setFeedback()
def get_joint_feedback(self):
""" Get the feedback from the joints.
Per joint: force-body1 (x,y,z); torque-body1 (x,y,z); frce-body2 (x,y,z); torque-body2 (x,y,z)"""
joint_feedback = []
for i in range(self._num_joints):
joint_feedback.append(self.man.joints[i].getFeedback())
return joint_feedback
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()]
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 get_joint_pos(self, jointidx):
assert (jointidx >= 0 and jointidx <= self._num_joints
), "Invalid joint index: %r" % jointidx
#return self.man.get_body_position(jointidx)
return self.man.get_uni_joint_position(jointidx, 1)
def get_joint_bodies(self, jointidx):
assert (jointidx >= 0 and jointidx <= self._num_joints
), "Invalid joint index: %r" % jointidx
return [
self.man.get_joint(jointidx).getBody(0),
self.man.get_joint(jointidx).getBody(0)
]
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 log_avg_bodyposition(self, timestamp):
avgbodypos = self.get_avg_position()
with open("avg_bodypos.out", "a") as myfile:
myfile.write( ` timestamp ` + ' ' + ` avgbodypos ` + '\n')
def log_bodysegmentcenter(self, timestamp, bodyidx):
bodypos = self.man.get_body_position(bodyidx)
with open("body_" + ` bodyidx ` + "_pos.out", "a") as myfile:
myfile.write( ` timestamp ` + ' ' + ` bodypos ` + '\n')