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 Quadruped(object):
""" Represent the quadruped robot. """
def __init__(self,man,base_pos=[0,0,0],morphology_genome={}):
""" 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)
"""
self.man = man
self.body_keys = []
# Sensors for robot.
self.sensor = Sensors(man)
# Hardware Limits
# Initialize the robot.
self.__create_robot(base_pos=base_pos,morphology=morphology_genome)
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
BODY_DIMS = morphology['body_dims'] if 'body_dims' in morphology else [3.0,0.5,1.0]
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 [0.0+base_pos[0],1.60+base_pos[1],0.0+base_pos[2]]
BODY_MASS = morphology['body_mass'] if 'body_mass' in morphology else 10.
# Upper Legs
F_UPP_DIMS = R_UPP_DIMS = morphology['u_l_dims'] if 'u_l_dims' in morphology else [0.5,.10]
F_UPP_MASS = R_UPP_MASS = morphology['u_l_mass'] if 'u_l_mass' in morphology else 2.
R_UPP_DIMS = [.5,.10]
R_UPP_MASS = 2.
# Custom Measurements Per Leg Group
# Front Upper Legs
if 'f_u_dims' in morphology:
F_UPP_DIMS = morphology['f_u_dims']
if 'f_u_mass' in morphology:
F_UPP_MASS = morphology['f_u_mass']
# Rear Upper Legs
if 'r_u_dims' in morphology:
R_UPP_DIMS = morphology['r_u_dims']
if 'r_u_mass' in morphology:
R_UPP_MASS = morphology['r_u_mass']
# Middle Legs
F_MID_DIMS = R_MID_DIMS = morphology['l_m_dims'] if 'l_m_dims' in morphology else [.75,.10]
F_MID_MASS = R_MID_MASS = morphology['l_m_mass'] if 'l_m_mass' in morphology else 1.
# Custom Measurements Per Leg Group
# Front Lower Legs
if 'f_m_dims' in morphology:
F_MID_DIMS = morphology['f_m_dims']
if 'f_m_mass' in morphology:
F_MID_MASS = morphology['f_m_mass']
# Rear Lower Legs
if 'r_m_dims' in morphology:
R_MID_DIMS = morphology['r_m_dims']
if 'r_m_mass' in morphology:
R_MID_MASS = morphology['r_m_mass']
# Joint Power for the legs
F_UPP_FORCE = R_UPP_FORCE = F_MID_FORCE = R_MID_FORCE = F_LOW_FORCE = R_LOW_FORCE = [80.,80.]
joint_range = math.radians(120.)
# Keep track of body and joint numbers.
b_num = 0
j_num = 0
# Create the Main Body
self.body_keys.append(self.man.create_box(b_num,BODY_DIMS,BODY_POS,density=BODY_MASS))
b_num += 1
# Create the Right Upper Front Leg
self.body_keys.append(self.man.create_capsule(b_num, F_UPP_MASS, F_UPP_DIMS[0], F_UPP_DIMS[1], [0.0,0.0,0.0],rot=[0.,0.,0.]))
#rot_t = [90.0,-60.0,0.0]
rot_t = [90.0,0.0,0.0]
con_point = [BODY_DIMS[0]/2.*.85,BODY_POS[1],BODY_DIMS[2]/2.+F_UPP_DIMS[1]/2.+.05]
Placement.place_capsule_at_trans(self.man.bodies[b_num],pos=con_point,rot=rot_t)
self.man.create_universal(j_num, con_point,[0,b_num],axis1=[0,0,-1],axis2=[1,0,0],loStop1=-joint_range,hiStop1=joint_range,loStop2=-joint_range,hiStop2=joint_range,fmax=F_UPP_FORCE[0],fmax2=F_UPP_FORCE[1])
b_num += 1
j_num += 1
# Create the Left Upper Front Leg
self.body_keys.append(self.man.create_capsule(b_num, F_UPP_MASS, F_UPP_DIMS[0], F_UPP_DIMS[1], [0.0,0.0,0.0],rot=[0.,0.,0.]))
# rot_t = [90.0,-60.0,0.0]
rot_t = [90.0,0.0,0.0]
con_point = [BODY_DIMS[0]/2.*.85,BODY_POS[1],-(BODY_DIMS[2]/2.+F_UPP_DIMS[1]/2.+.05)]
Placement.place_capsule_at_trans(self.man.bodies[b_num],pos=con_point,rot=rot_t)
self.man.create_universal(j_num, con_point,[0,b_num],axis1=[0,0,-1],axis2=[-1,0,0],loStop1=-joint_range,hiStop1=joint_range,loStop2=-joint_range,hiStop2=joint_range,fmax=F_UPP_FORCE[0],fmax2=F_UPP_FORCE[1])
b_num += 1
j_num += 1
# Create the Right Upper Rear Leg
self.body_keys.append(self.man.create_capsule(b_num, R_UPP_MASS, R_UPP_DIMS[0], R_UPP_DIMS[1], [0.0,0.0,0.0],rot=[0.,0.,0.]))
# rot_t = [90.0,60.0,0.0]
rot_t = [90.0,0.0,0.0]
con_point = [-BODY_DIMS[0]/2.*.8,BODY_POS[1],BODY_DIMS[2]/2.+R_UPP_DIMS[1]/2.+0.05]
Placement.place_capsule_at_trans(self.man.bodies[b_num],pos=con_point,rot=rot_t)
self.man.create_universal(j_num, con_point,[0,b_num],axis1=[0,0,1],axis2=[1,0,0],loStop1=-joint_range,hiStop1=joint_range,loStop2=-joint_range,hiStop2=joint_range,fmax=F_UPP_FORCE[0],fmax2=F_UPP_FORCE[1])
b_num += 1
j_num += 1
# Create the Left Upper Rear Leg
self.body_keys.append(self.man.create_capsule(b_num, R_UPP_MASS, R_UPP_DIMS[0], R_UPP_DIMS[1], [0.0,0.0,0.0],rot=[0.,0.,0.]))
# rot_t = [90.0,60.0,0.0]
rot_t = [90.0,0.0,0.0]
con_point = [-BODY_DIMS[0]/2.*.8,BODY_POS[1],-(BODY_DIMS[2]/2.+R_UPP_DIMS[1]/2.+0.05)]
Placement.place_capsule_at_trans(self.man.bodies[b_num],pos=con_point,rot=rot_t)
self.man.create_universal(j_num, con_point,[0,b_num],axis1=[0,0,1],axis2=[-1,0,0],loStop1=-joint_range,hiStop1=joint_range,loStop2=-joint_range,hiStop2=joint_range,fmax=F_UPP_FORCE[0],fmax2=F_UPP_FORCE[1])
b_num += 1
j_num += 1
# Create the Right Lower Front Leg
self.body_keys.append(self.man.create_capsule(b_num, F_MID_MASS, F_MID_DIMS[0], F_MID_DIMS[1], [0.0,0.0,0.0],rot=[0.,0.,0.]))
# rot_t = [0.0,135.0,0.0]
rot_t = [0.0,0.0,0.0]
con_point = Placement.place_capsule_trans(self.man.bodies[b_num-4],self.man.bodies[b_num],rot=rot_t)
self.man.create_universal(j_num, con_point,[b_num-4,b_num],axis1=[0,0,-1],axis2=[1,0,0],loStop1=-joint_range,hiStop1=joint_range,loStop2=-joint_range,hiStop2=joint_range,fmax=F_LOW_FORCE[0],fmax2=F_LOW_FORCE[1])
b_num += 1
j_num += 1
# Create the Left Lower Front Leg
self.body_keys.append(self.man.create_capsule(b_num, F_MID_MASS, F_MID_DIMS[0], F_MID_DIMS[1], [0.0,0.0,0.0],rot=[0.,0.,0.]))
# rot_t = [0.0,135.0,0.0]
rot_t = [0.0,0.0,0.0]
con_point = Placement.place_capsule_trans(self.man.bodies[b_num-4],self.man.bodies[b_num],rot=rot_t)
self.man.create_universal(j_num, con_point,[b_num-4,b_num],axis1=[0,0,-1],axis2=[-1,0,0],loStop1=-joint_range,hiStop1=joint_range,loStop2=-joint_range,hiStop2=joint_range,fmax=F_LOW_FORCE[0],fmax2=F_LOW_FORCE[1])
b_num += 1
j_num += 1
# Create the Right Lower Rear Leg
self.body_keys.append(self.man.create_capsule(b_num, R_MID_MASS, R_MID_DIMS[0], R_MID_DIMS[1], [0.0,0.0,0.0],rot=[0.,0.,0.]))
# rot_t = [0.0,-135.0,0.0]
rot_t = [0.0,0.0,0.0]
con_point = Placement.place_capsule_trans(self.man.bodies[b_num-4],self.man.bodies[b_num],rot=rot_t)
self.man.create_universal(j_num, con_point,[b_num-4,b_num],axis1=[0,0,1],axis2=[1,0,0],loStop1=-joint_range,hiStop1=joint_range,loStop2=-joint_range,hiStop2=joint_range,fmax=F_LOW_FORCE[0],fmax2=F_LOW_FORCE[1])
b_num += 1
j_num += 1
# Create the Left Lower Rear Leg
self.body_keys.append(self.man.create_capsule(b_num, R_MID_MASS, R_MID_DIMS[0], R_MID_DIMS[1], [0.0,0.0,0.0],rot=[0.,0.,0.]))
# rot_t = [0.0,-135.0,0.0]
rot_t = [0.0,0.0,0.0]
con_point = Placement.place_capsule_trans(self.man.bodies[b_num-4],self.man.bodies[b_num],rot=rot_t)
self.man.create_universal(j_num, con_point,[b_num-4,b_num],axis1=[0,0,1],axis2=[-1,0,0],loStop1=-joint_range,hiStop1=joint_range,loStop2=-joint_range,hiStop2=joint_range,fmax=F_LOW_FORCE[0],fmax2=F_LOW_FORCE[1])
b_num += 1
j_num += 1
# Add in information about the feet.
self.sensor.add_touch_sensor([5,6,7,8])
# Add in joint positions sensors.
self.sensor.register_joint_sensors([0,1,2,3,4,5,6,7])
def actuate_joints_by_pos(self,positions=[[0.,0.] for i in xrange(8)]):
""" Actuate the joints of the quadruped to the specified position.
Arguments:
positions: position of the joints to actuate to.
"""
for i,p in enumerate(positions):
self.man.actuate_universal(i,p[0],p[1])
def get_sensor_states(self):
""" Get the states of the various sensors on the robot. """
sensors = [i for i in self.sensor.get_touch_sensor_states()]
sensors += [i for i in self.sensor.get_joint_sensors()]
return sensors
def reset_touch_sensors(self):
""" Reset the touch sensors. """
self.sensor.clear_touching()
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, 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')
class Quadruped(object):
""" Represent the quadruped robot. """
def __init__(self, man, base_pos=[0, 0, 0], morphology_genome={}):
""" 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)
"""
self.man = man
self.body_keys = []
# Sensors for robot.
self.sensor = Sensors(man)
# Hardware Limits
# Initialize the robot.
self.__create_robot(base_pos=base_pos, morphology=morphology_genome)
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
BODY_DIMS = morphology['body_dims'] if 'body_dims' in morphology else [
3.0, 0.5, 1.0
]
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 [0.0+base_pos[0],1.60+base_pos[1],0.0+base_pos[2]]
BODY_MASS = morphology[
'body_mass'] if 'body_mass' in morphology else 10.
# Upper Legs
F_UPP_DIMS = R_UPP_DIMS = morphology[
'u_l_dims'] if 'u_l_dims' in morphology else [0.5, .10]
F_UPP_MASS = R_UPP_MASS = morphology[
'u_l_mass'] if 'u_l_mass' in morphology else 2.
R_UPP_DIMS = [.5, .10]
R_UPP_MASS = 2.
# Custom Measurements Per Leg Group
# Front Upper Legs
if 'f_u_dims' in morphology:
F_UPP_DIMS = morphology['f_u_dims']
if 'f_u_mass' in morphology:
F_UPP_MASS = morphology['f_u_mass']
# Rear Upper Legs
if 'r_u_dims' in morphology:
R_UPP_DIMS = morphology['r_u_dims']
if 'r_u_mass' in morphology:
R_UPP_MASS = morphology['r_u_mass']
# Middle Legs
F_MID_DIMS = R_MID_DIMS = morphology[
'l_m_dims'] if 'l_m_dims' in morphology else [.75, .10]
F_MID_MASS = R_MID_MASS = morphology[
'l_m_mass'] if 'l_m_mass' in morphology else 1.
# Custom Measurements Per Leg Group
# Front Lower Legs
if 'f_m_dims' in morphology:
F_MID_DIMS = morphology['f_m_dims']
if 'f_m_mass' in morphology:
F_MID_MASS = morphology['f_m_mass']
# Rear Lower Legs
if 'r_m_dims' in morphology:
R_MID_DIMS = morphology['r_m_dims']
if 'r_m_mass' in morphology:
R_MID_MASS = morphology['r_m_mass']
# Joint Power for the legs
F_UPP_FORCE = R_UPP_FORCE = F_MID_FORCE = R_MID_FORCE = F_LOW_FORCE = R_LOW_FORCE = [
80., 80.
]
joint_range = math.radians(120.)
# Keep track of body and joint numbers.
b_num = 0
j_num = 0
# Create the Main Body
self.body_keys.append(
self.man.create_box(b_num, BODY_DIMS, BODY_POS, density=BODY_MASS))
b_num += 1
# Create the Right Upper Front Leg
self.body_keys.append(
self.man.create_capsule(b_num,
F_UPP_MASS,
F_UPP_DIMS[0],
F_UPP_DIMS[1], [0.0, 0.0, 0.0],
rot=[0., 0., 0.]))
#rot_t = [90.0,-60.0,0.0]
rot_t = [90.0, 0.0, 0.0]
con_point = [
BODY_DIMS[0] / 2. * .85, BODY_POS[1],
BODY_DIMS[2] / 2. + F_UPP_DIMS[1] / 2. + .05
]
Placement.place_capsule_at_trans(self.man.bodies[b_num],
pos=con_point,
rot=rot_t)
self.man.create_universal(j_num,
con_point, [0, b_num],
axis1=[0, 0, -1],
axis2=[1, 0, 0],
loStop1=-joint_range,
hiStop1=joint_range,
loStop2=-joint_range,
hiStop2=joint_range,
fmax=F_UPP_FORCE[0],
fmax2=F_UPP_FORCE[1])
b_num += 1
j_num += 1
# Create the Left Upper Front Leg
self.body_keys.append(
self.man.create_capsule(b_num,
F_UPP_MASS,
F_UPP_DIMS[0],
F_UPP_DIMS[1], [0.0, 0.0, 0.0],
rot=[0., 0., 0.]))
# rot_t = [90.0,-60.0,0.0]
rot_t = [90.0, 0.0, 0.0]
con_point = [
BODY_DIMS[0] / 2. * .85, BODY_POS[1],
-(BODY_DIMS[2] / 2. + F_UPP_DIMS[1] / 2. + .05)
]
Placement.place_capsule_at_trans(self.man.bodies[b_num],
pos=con_point,
rot=rot_t)
self.man.create_universal(j_num,
con_point, [0, b_num],
axis1=[0, 0, -1],
axis2=[-1, 0, 0],
loStop1=-joint_range,
hiStop1=joint_range,
loStop2=-joint_range,
hiStop2=joint_range,
fmax=F_UPP_FORCE[0],
fmax2=F_UPP_FORCE[1])
b_num += 1
j_num += 1
# Create the Right Upper Rear Leg
self.body_keys.append(
self.man.create_capsule(b_num,
R_UPP_MASS,
R_UPP_DIMS[0],
R_UPP_DIMS[1], [0.0, 0.0, 0.0],
rot=[0., 0., 0.]))
# rot_t = [90.0,60.0,0.0]
rot_t = [90.0, 0.0, 0.0]
con_point = [
-BODY_DIMS[0] / 2. * .8, BODY_POS[1],
BODY_DIMS[2] / 2. + R_UPP_DIMS[1] / 2. + 0.05
]
Placement.place_capsule_at_trans(self.man.bodies[b_num],
pos=con_point,
rot=rot_t)
self.man.create_universal(j_num,
con_point, [0, b_num],
axis1=[0, 0, 1],
axis2=[1, 0, 0],
loStop1=-joint_range,
hiStop1=joint_range,
loStop2=-joint_range,
hiStop2=joint_range,
fmax=F_UPP_FORCE[0],
fmax2=F_UPP_FORCE[1])
b_num += 1
j_num += 1
# Create the Left Upper Rear Leg
self.body_keys.append(
self.man.create_capsule(b_num,
R_UPP_MASS,
R_UPP_DIMS[0],
R_UPP_DIMS[1], [0.0, 0.0, 0.0],
rot=[0., 0., 0.]))
# rot_t = [90.0,60.0,0.0]
rot_t = [90.0, 0.0, 0.0]
con_point = [
-BODY_DIMS[0] / 2. * .8, BODY_POS[1],
-(BODY_DIMS[2] / 2. + R_UPP_DIMS[1] / 2. + 0.05)
]
Placement.place_capsule_at_trans(self.man.bodies[b_num],
pos=con_point,
rot=rot_t)
self.man.create_universal(j_num,
con_point, [0, b_num],
axis1=[0, 0, 1],
axis2=[-1, 0, 0],
loStop1=-joint_range,
hiStop1=joint_range,
loStop2=-joint_range,
hiStop2=joint_range,
fmax=F_UPP_FORCE[0],
fmax2=F_UPP_FORCE[1])
b_num += 1
j_num += 1
# Create the Right Lower Front Leg
self.body_keys.append(
self.man.create_capsule(b_num,
F_MID_MASS,
F_MID_DIMS[0],
F_MID_DIMS[1], [0.0, 0.0, 0.0],
rot=[0., 0., 0.]))
# rot_t = [0.0,135.0,0.0]
rot_t = [0.0, 0.0, 0.0]
con_point = Placement.place_capsule_trans(self.man.bodies[b_num - 4],
self.man.bodies[b_num],
rot=rot_t)
self.man.create_universal(j_num,
con_point, [b_num - 4, b_num],
axis1=[0, 0, -1],
axis2=[1, 0, 0],
loStop1=-joint_range,
hiStop1=joint_range,
loStop2=-joint_range,
hiStop2=joint_range,
fmax=F_LOW_FORCE[0],
fmax2=F_LOW_FORCE[1])
b_num += 1
j_num += 1
# Create the Left Lower Front Leg
self.body_keys.append(
self.man.create_capsule(b_num,
F_MID_MASS,
F_MID_DIMS[0],
F_MID_DIMS[1], [0.0, 0.0, 0.0],
rot=[0., 0., 0.]))
# rot_t = [0.0,135.0,0.0]
rot_t = [0.0, 0.0, 0.0]
con_point = Placement.place_capsule_trans(self.man.bodies[b_num - 4],
self.man.bodies[b_num],
rot=rot_t)
self.man.create_universal(j_num,
con_point, [b_num - 4, b_num],
axis1=[0, 0, -1],
axis2=[-1, 0, 0],
loStop1=-joint_range,
hiStop1=joint_range,
loStop2=-joint_range,
hiStop2=joint_range,
fmax=F_LOW_FORCE[0],
fmax2=F_LOW_FORCE[1])
b_num += 1
j_num += 1
# Create the Right Lower Rear Leg
self.body_keys.append(
self.man.create_capsule(b_num,
R_MID_MASS,
R_MID_DIMS[0],
R_MID_DIMS[1], [0.0, 0.0, 0.0],
rot=[0., 0., 0.]))
# rot_t = [0.0,-135.0,0.0]
rot_t = [0.0, 0.0, 0.0]
con_point = Placement.place_capsule_trans(self.man.bodies[b_num - 4],
self.man.bodies[b_num],
rot=rot_t)
self.man.create_universal(j_num,
con_point, [b_num - 4, b_num],
axis1=[0, 0, 1],
axis2=[1, 0, 0],
loStop1=-joint_range,
hiStop1=joint_range,
loStop2=-joint_range,
hiStop2=joint_range,
fmax=F_LOW_FORCE[0],
fmax2=F_LOW_FORCE[1])
b_num += 1
j_num += 1
# Create the Left Lower Rear Leg
self.body_keys.append(
self.man.create_capsule(b_num,
R_MID_MASS,
R_MID_DIMS[0],
R_MID_DIMS[1], [0.0, 0.0, 0.0],
rot=[0., 0., 0.]))
# rot_t = [0.0,-135.0,0.0]
rot_t = [0.0, 0.0, 0.0]
con_point = Placement.place_capsule_trans(self.man.bodies[b_num - 4],
self.man.bodies[b_num],
rot=rot_t)
self.man.create_universal(j_num,
con_point, [b_num - 4, b_num],
axis1=[0, 0, 1],
axis2=[-1, 0, 0],
loStop1=-joint_range,
hiStop1=joint_range,
loStop2=-joint_range,
hiStop2=joint_range,
fmax=F_LOW_FORCE[0],
fmax2=F_LOW_FORCE[1])
b_num += 1
j_num += 1
# Add in information about the feet.
self.sensor.add_touch_sensor([5, 6, 7, 8])
# Add in joint positions sensors.
self.sensor.register_joint_sensors([0, 1, 2, 3, 4, 5, 6, 7])
def actuate_joints_by_pos(self, positions=[[0., 0.] for i in xrange(8)]):
""" Actuate the joints of the quadruped to the specified position.
Arguments:
positions: position of the joints to actuate to.
"""
for i, p in enumerate(positions):
self.man.actuate_universal(i, p[0], p[1])
def get_sensor_states(self):
""" Get the states of the various sensors on the robot. """
sensors = [i for i in self.sensor.get_touch_sensor_states()]
sensors += [i for i in self.sensor.get_joint_sensors()]
return sensors
def reset_touch_sensors(self):
""" Reset the touch sensors. """
self.sensor.clear_touching()
class Worm(object):
""" Represent the worm robot. """
def __init__(self,
man,
base_pos=[0, 0, 0],
morphology_genome={},
num_joints=2):
""" 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 = []
# Sensors for robot.
self.sensor = Sensors(man)
self._num_joints = num_joints
# Hardware Limits
# Initialize the robot.
self.__create_robot(base_pos=base_pos, morphology=morphology_genome)
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
WORM_LENGTH = 10.
SEG_DIMS = morphology['seg_dims'] if 'seg_dims' in morphology else [
WORM_LENGTH / (self._num_joints + 1), .50, .50
]
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]]
BODY_MASS = morphology['body_mass'] if 'body_mass' in morphology else 5.
# Joint Power for the legs
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 information about the feet.
self.sensor.add_touch_sensor([i for i in range(b_num)])
# Add in joint positions sensors.
self.sensor.register_joint_sensors([i for i in range(j_num)])
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.
"""
for i, p in enumerate(positions):
self.man.actuate_universal(i, p[0], p[1])
def get_sensor_states(self):
""" Get the states of the various sensors on the robot. """
sensors = [i for i in self.sensor.get_touch_sensor_states()]
sensors += [i for i in self.sensor.get_joint_sensors()]
return sensors
def reset_touch_sensors(self):
""" Reset the touch sensors. """
self.sensor.clear_touching()
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, man.get_body_position(i))]
# Average the positions.
avg_pos = [i / (self._num_joints + 1) for i in avg_pos]
return avg_pos