def set_door_friction(self, friction):
node = self.objects[0].find("./body[@name='frame']")
node = node.find("./body[@name='door']")
hinge = node.find("./joint[@name='door_hinge']")
hinge.set("frictionloss", array_to_string(np.array([friction])))
def set_door_damping(self, damping):
hinge = self._base_body.find("./joint[@name='door_hinge']")
node = self.objects[0].find("./body[@name='frame']")
node = node.find("./body[@name='door']")
hinge = node.find("./joint[@name='door_hinge']")
hinge.set("damping", array_to_string(np.array([damping])))
def _get_visual(self, name=None, site=False, ob_type="box"):
main_body = ET.Element("body")
if name is not None:
main_body.set("name", name)
template = self.get_visual_attrib_template()
template["type"] = ob_type
template["rgba"] = array_to_string(self.rgba)
template["size"] = array_to_string(self.size)
main_body.append(ET.Element("geom", attrib=template))
if site:
# add a site as well
template = self.get_site_attrib_template()
if name is not None:
template["name"] = name
main_body.append(ET.Element("site", attrib=template))
return main_body
def set_origin(self, offset):
"""Applies a constant offset to all objects."""
offset = np.array(offset)
for node in self.worldbody.findall("./*[@pos]"):
cur_pos = string_to_array(node.get("pos"))
new_pos = cur_pos + offset
node.set("pos", array_to_string(new_pos))
def set_joint_frictionloss(self,
friction=np.array(
(0.1, 0.1, 0.1, 0.1, 0.1, 0.01, 0.01))):
"""Set joint friction loss (static friction)"""
body = self._base_body
for i in range(len(self._link_body)):
body = body.find("./body[@name='{}']".format(self._link_body[i]))
joint = body.find("./joint[@name='{}']".format(self._joints[i]))
joint.set("frictionloss", array_to_string(np.array([friction[i]])))
def set_joint_damping(self,
damping=np.array(
(0.1, 0.1, 0.1, 0.1, 0.1, 0.01, 0.01))):
"""Set joint damping """
body = self._base_body
for i in range(len(self._link_body)):
body = body.find("./body[@name='{}']".format(self._link_body[i]))
joint = body.find("./joint[@name='{}']".format(self._joints[i]))
joint.set("damping", array_to_string(np.array([damping[i]])))
def place_objects(self, randomize=False):
"""Places objects randomly until no collisions or max iterations hit."""
# pos_arr = [0.762, 0.0, 1.43] //for big door
# pos_arr = [0.762, 0.0, 1.00]
pos_arr = [0.85, 0.30, 1.45]
if randomize == True:
x, y = np.random.uniform(high=np.array([0.05, 0.05]),
low=np.array([-0.05, -0.05]))
degree = np.random.uniform(high=10, low=-10)
pos_arr[0] += x
pos_arr[1] += y
else:
degree = 0.0
rot_angle = degree * np.pi / 180.0
quat = [np.cos(rot_angle / 2), 0, 0, np.sin(rot_angle / 2)]
index = 0
for k, obj_name in enumerate(self.mujoco_objects):
# print(array_to_string(pos_arr))
self.objects[index].set("pos", array_to_string(pos_arr))
self.objects[index].set("quat", array_to_string(quat))
def configure_location(self):
self.bottom_pos = np.array([0, 0, 0])
self.floor.set("pos", array_to_string(self.bottom_pos))
self.center_pos = self.bottom_pos + np.array([0, 0, self.table_half_size[2]])
self.table_body.set("pos", array_to_string(self.center_pos))
self.table_collision.set("size", array_to_string(self.table_half_size))
self.table_collision.set("friction", array_to_string(self.table_friction))
self.table_visual.set("size", array_to_string(self.table_half_size))
self.table_top.set(
"pos", array_to_string(np.array([0, 0, self.table_half_size[2]]))
)
def place_objects(self):
"""Places objects randomly until no collisions or max iterations hit."""
pos_arr, quat_arr = self.initializer.sample()
for k, obj_name in enumerate(self.objects):
self.objects[obj_name].set("pos", array_to_string(pos_arr[k]))
self.objects[obj_name].set("quat", array_to_string(quat_arr[k]))
def place_objects(self):
"""Places objects randomly until no collisions or max iterations hit."""
pos_arr, quat_arr = self.initializer.sample()
for i in range(len(self.objects)):
self.objects[i].set("pos", array_to_string(pos_arr[i]))
self.objects[i].set("quat", array_to_string(quat_arr[i]))
def configure_location(self):
self.bottom_pos = np.array([0, 0, 0])
self.floor.set("pos", array_to_string(self.bottom_pos))
def configure_location(self):
self.bottom_pos = np.array([0, 0, 0])
self.floor.set("pos", array_to_string(self.bottom_pos))
self.center_pos = self.bottom_pos + np.array(
[0, 0, self.table_half_size[2]])
self.table_body.set("pos", array_to_string(self.center_pos))
self.table_collision.set("size", array_to_string(self.table_half_size))
self.table_collision.set("friction",
array_to_string(self.table_friction))
self.table_visual.set("size", array_to_string(self.table_half_size))
#Compute size of the squares
self.square_full_size = np.divide(self.table_full_size[0:2],
self.num_squares)
self.square_half_size = np.divide(self.table_half_size[0:2],
self.num_squares)
self.squares = OrderedDict()
for i in range(self.num_squares[0]):
for j in range(self.num_squares[1]):
self.mujoco_objects = OrderedDict()
square = BoxObject(size=[
self.square_half_size[0] - 0.0005,
self.square_half_size[1] - 0.0005, 0.001
],
rgba=[0, 0, 1, 1],
density=500,
friction=0.05)
square_name = 'contact_' + str(i) + "_" + str(j)
self.squares[square_name] = square
table_subtree = self.worldbody.find(
".//body[@name='{}']".format("table"))
collision_b = square.get_collision(name=square_name, site=True)
collision_b.set(
"pos",
array_to_string([
self.table_half_size[0] -
i * self.square_full_size[0] -
0.5 * self.square_full_size[0],
self.table_half_size[1] -
j * self.square_full_size[1] -
0.5 * self.square_full_size[1],
self.table_half_size[2] + 0.05
]))
collision_b.find("site").set("pos",
array_to_string([0, 0, 0.002]))
collision_b.find("site").set(
"pos",
array_to_string([
self.table_half_size[0] -
i * self.square_full_size[0] -
0.5 * self.square_full_size[0],
self.table_half_size[1] -
j * self.square_full_size[1] -
0.5 * self.square_full_size[1],
self.table_half_size[2] + 0.005
]))
table_subtree.append(collision_b.find("site"))
#print(ET.tostring(collision_b.find("site")))
# square2 = BoxObject(size=[self.square_half_size[0]-0.0005, self.square_half_size[1]-0.0005, 0.03/2 - 0.001],
# rgba=[1, 0, 0, 1],
# density=500,
# friction=0.05)
# collision_c = square2.get_collision(name=square_name, site=True)
# #print(ET.tostring(collision_c))
# #print(50*'dd')
# collision_c.set("pos", array_to_string([
# self.table_half_size[0]-i*self.square_full_size[0]-0.5*self.square_full_size[0],
# self.table_half_size[1]-j*self.square_full_size[1]-0.5*self.square_full_size[1],
# self.table_half_size[2]+0.03/2 - 0.001]))
# collision_c.find("site").set("pos", array_to_string([0,0,0.03/2]))
# collision_c.find("site").set("size", array_to_string([self.square_half_size[0]-0.0005, self.square_half_size[1]-0.0005,0.002]))
# collision_c.find("site").set("rgba", array_to_string([0,0,1, 1]))
# table_subtree.append(collision_c)
#print(ET.tostring(collision_c))
ET.SubElement(self.sensor,
"touch",
attrib={
"name": square_name + "_sensor",
"site": square_name
}) # + "_site"})
#print(self.get_xml())
#exit()
self.table_top.set(
"pos", array_to_string(np.array([0, 0, self.table_half_size[2]])))
def set_base_xpos(self, pos):
"""Places the robot on position @pos."""
node = self.worldbody.find("./body[@name='link0']")
node.set("pos", array_to_string(pos - self.bottom_offset))
def configure_location(self):
self.bottom_pos = np.array([0, 0, 0])
self.floor.set("pos", array_to_string(self.bottom_pos))
self.center_pos = self.bottom_pos + np.array(
[0, 0, self.table_half_size[2]])
self.table_body.set("pos", array_to_string(self.center_pos))
qx = Quaternion(axis=(1.0, 0.0, 0.0), radians=self.rotation_x)
qy = Quaternion(axis=(0.0, 1.0, 0.0), radians=self.rotation_y)
qt = qy * qx
friction = max(
0.001,
np.random.normal(self.table_friction[0], self.table_friction_std))
print("New table friction:" + str(friction))
self.table_body.set("quat", array_to_string(qt.elements))
self.table_collision.set("size", array_to_string(self.table_half_size))
self.table_collision.set("friction",
array_to_string(self.table_friction))
self.table_visual.set("size", array_to_string(self.table_half_size))
#Compute size of the squares
self.square_full_size = np.divide(
self.table_full_size[0:2] * self.coverage_factor, self.num_squares)
self.square_half_size = self.square_full_size / 2.0
self.peg_size = 0.01
if self.draw_line:
self.squares = OrderedDict()
self.mujoco_objects = OrderedDict()
table_subtree = self.worldbody.find(
".//body[@name='{}']".format("table"))
# Sites on additional bodies attached to the table
#squares_separation = 0.0005
squares_separation = 0.0
squares_height = 0.1
table_cte_size_x = 0.8
table_cte_size_y = 1.0
square2 = BoxObject(size=[
table_cte_size_x / 2, table_cte_size_y / 2,
squares_height / 2 - 0.001
],
rgba=[0, 0, 1, 1],
density=1,
friction=friction)
square_name2 = 'table_0_0'
self.squares[square_name2] = square2
collision_c = square2.get_collision(name=square_name2, site=True)
#Align the constant sized table to the bottom of the table
collision_c.set(
"pos",
array_to_string([
table_cte_size_x / 2 - self.table_half_size[0] - 0.18, 0,
self.table_half_size[2] + squares_height / 2
]))
table_subtree.append(collision_c)
pos = np.array((np.random.uniform(-self.table_half_size[0],
self.table_half_size[0]),
np.random.uniform(-self.table_half_size[1],
self.table_half_size[1])))
direction = np.random.uniform(-np.pi, np.pi)
if not self.two_clusters:
for i in range(self.num_sensors):
square2 = CylinderObject(size=[self.line_width, 0.001],
rgba=[0, 1, 0, 0],
density=1,
friction=friction)
square_name2 = 'contact_' + str(i)
self.squares[square_name2] = square2
collision_c = square2.get_collision(name=square_name2,
site=True)
collision_c.set(
"pos",
array_to_string([
pos[0], pos[1],
self.table_half_size[2] + squares_height
]))
#collision_c.find("site").set("pos", array_to_string([0,0,0]))
collision_c.find("site").set(
"pos",
array_to_string([
pos[0], pos[1],
self.table_half_size[2] + squares_height + 0.005
]))
collision_c.find("site").set(
"size", array_to_string([self.line_width, 0.001]))
collision_c.find("geom").set("contype", "0")
collision_c.find("geom").set("conaffinity", "0")
collision_c.find("site").set("rgba",
array_to_string([0, 1, 0, 1]))
#table_subtree.append(collision_c)
table_subtree.append(collision_c.find("site"))
sensor_name = square_name2 + "_sensor"
sensor_site_name = square_name2 # +"_site"
self.sensor_names += [sensor_name]
self.sensor_site_names[sensor_name] = sensor_site_name
#ET.SubElement(self.sensor, "touch", attrib={"name" : sensor_name, "site" : sensor_site_name})
if np.random.uniform(0, 1) > 0.7:
direction += np.random.normal(0, 0.5)
posnew0 = pos[0] + 0.005 * np.sin(direction)
posnew1 = pos[1] + 0.005 * np.cos(direction)
while abs(posnew0) >= self.table_half_size[0] or abs(
posnew1) >= self.table_half_size[1]:
direction += np.random.normal(0, 0.5)
posnew0 = pos[0] + 0.005 * np.sin(direction)
posnew1 = pos[1] + 0.005 * np.cos(direction)
pos[0] = posnew0
pos[1] = posnew1
else:
half_num_sensors = int(np.floor(self.num_sensors / 2))
last_i = 0
for i in range(half_num_sensors):
square2 = CylinderObject(size=[self.line_width, 0.001],
rgba=[0, 1, 0, 0],
density=1,
friction=friction)
square_name2 = 'contact_' + str(i)
self.squares[square_name2] = square2
collision_c = square2.get_collision(name=square_name2,
site=True)
collision_c.set(
"pos",
array_to_string([
pos[0], pos[1],
self.table_half_size[2] + squares_height + 0.01
]))
#collision_c.find("site").set("pos", array_to_string([0,0,0]))
collision_c.find("site").set(
"pos",
array_to_string([
pos[0], pos[1],
self.table_half_size[2] + squares_height + 0.005
]))
collision_c.find("site").set(
"size", array_to_string([self.line_width, 0.001]))
collision_c.find("geom").set("contype", "0")
collision_c.find("geom").set("conaffinity", "0")
collision_c.find("site").set("rgba",
array_to_string([0, 1, 0, 1]))
#table_subtree.append(collision_c)
table_subtree.append(collision_c.find("site"))
sensor_name = square_name2 + "_sensor"
sensor_site_name = square_name2 # +"_site"
self.sensor_names += [sensor_name]
self.sensor_site_names[sensor_name] = sensor_site_name
#ET.SubElement(self.sensor, "touch", attrib={"name" : sensor_name, "site" : sensor_site_name})
if np.random.uniform(0, 1) > 0.7:
direction += np.random.normal(0, 0.5)
posnew0 = pos[0] + 0.005 * np.sin(direction)
posnew1 = pos[1] + 0.005 * np.cos(direction)
while abs(posnew0) >= self.table_half_size[0] or abs(
posnew1) >= self.table_half_size[1]:
direction += np.random.normal(0, 0.5)
posnew0 = pos[0] + 0.005 * np.sin(direction)
posnew1 = pos[1] + 0.005 * np.cos(direction)
pos[0] = posnew0
pos[1] = posnew1
last_i = i
last_i += 1
pos = np.array((np.random.uniform(-self.table_half_size[0],
self.table_half_size[0]),
np.random.uniform(-self.table_half_size[1],
self.table_half_size[1])))
direction = np.random.uniform(-np.pi, np.pi)
for i in range(half_num_sensors):
square2 = CylinderObject(size=[self.line_width, 0.001],
rgba=[0, 1, 0, 0],
density=1,
friction=friction)
square_name2 = 'contact_' + str(last_i + i)
self.squares[square_name2] = square2
collision_c = square2.get_collision(name=square_name2,
site=True)
collision_c.set(
"pos",
array_to_string([
pos[0], pos[1],
self.table_half_size[2] + squares_height + 0.01
]))
#collision_c.find("site").set("pos", array_to_string([0,0,0]))
collision_c.find("site").set(
"pos",
array_to_string([
pos[0], pos[1],
self.table_half_size[2] + squares_height + 0.005
]))
collision_c.find("site").set(
"size", array_to_string([self.line_width, 0.001]))
collision_c.find("geom").set("contype", "0")
collision_c.find("geom").set("conaffinity", "0")
collision_c.find("site").set("rgba",
array_to_string([0, 1, 0, 1]))
#table_subtree.append(collision_c)
table_subtree.append(collision_c.find("site"))
sensor_name = square_name2 + "_sensor"
sensor_site_name = square_name2 # +"_site"
self.sensor_names += [sensor_name]
self.sensor_site_names[sensor_name] = sensor_site_name
#ET.SubElement(self.sensor, "touch", attrib={"name" : sensor_name, "site" : sensor_site_name})
if np.random.uniform(0, 1) > 0.7:
direction += np.random.normal(0, 0.5)
posnew0 = pos[0] + 0.005 * np.sin(direction)
posnew1 = pos[1] + 0.005 * np.cos(direction)
while abs(posnew0) >= self.table_half_size[0] or abs(
posnew1) >= self.table_half_size[1]:
direction += np.random.normal(0, 0.5)
posnew0 = pos[0] + 0.005 * np.sin(direction)
posnew1 = pos[1] + 0.005 * np.cos(direction)
pos[0] = posnew0
pos[1] = posnew1
else:
self.squares = OrderedDict()
indices = [
a for a in itertools.product(range(self.num_squares[0]),
range(self.num_squares[1]))
]
picked_indices = random.sample(
indices, int(np.ceil(self.prob_sensor * len(indices))))
for i in range(self.num_squares[0]):
for j in range(self.num_squares[1]):
self.mujoco_objects = OrderedDict()
table_subtree = self.worldbody.find(
".//body[@name='{}']".format("table"))
# Sites directly attached to the table
# square = BoxObject(size=[self.square_half_size[0]-0.0005, self.square_half_size[1]-0.0005, 0.001],
# rgba=[0, 0, 1, 1],
# density=500,
# friction=0.05)
# square_name = 'contact_'+str(i) + "_" + str(j)
# self.squares[square_name] = square
# collision_b = square.get_collision(name=square_name, site=True)
# collision_b.set("pos", array_to_string([
# self.table_half_size[0]-i*self.square_full_size[0]-0.5*self.square_full_size[0],
# self.table_half_size[1]-j*self.square_full_size[1]-0.5*self.square_full_size[1],
# self.table_half_size[2]+0.05]))
# collision_b.find("site").set("pos", array_to_string([0,0,0.002]))
# collision_b.find("site").set("pos", array_to_string([
# self.table_half_size[0]-i*self.square_full_size[0]-0.5*self.square_full_size[0],
# self.table_half_size[1]-j*self.square_full_size[1]-0.5*self.square_full_size[1],
# self.table_half_size[2]+0.005]))
# table_subtree.append(collision_b.find("site"))
# sensor_name = square_name +"_sensor"
# self.sensor_names += [sensor_name]
# ET.SubElement(self.sensor, "force", attrib={"name" : sensor_name, "site" : square_name + "_site"})
#print(ET.tostring(collision_b.find("site")))
# Sites on additional bodies attached to the table
#squares_separation = 0.0005
squares_separation = 0.0
squares_height = 0.1
square2 = BoxObject(size=[
self.square_half_size[0] - 0.0005,
self.square_half_size[1] - squares_separation,
squares_height / 2 - 0.001
],
rgba=[0, 1, 0, 1],
density=1,
friction=0.0001)
square_name2 = 'contact_' + str(i) + "_" + str(j)
self.squares[square_name2] = square2
collision_c = square2.get_collision(name=square_name2,
site=True)
collision_c.set(
"pos",
array_to_string([
self.table_half_size[0] -
i * self.square_full_size[0] -
0.5 * self.square_full_size[0],
self.table_half_size[1] -
j * self.square_full_size[1] -
0.5 * self.square_full_size[1],
self.table_half_size[2] + squares_height / 2 -
0.001
]))
collision_c.find("site").set(
"pos", array_to_string([0, 0, squares_height / 2]))
collision_c.find("site").set(
"size",
array_to_string([
self.square_half_size[0] - squares_separation,
self.square_half_size[1] - squares_separation,
0.002
]))
place_this_sensor = (i, j) in picked_indices
if i < self.num_squares[0] - 1 and place_this_sensor:
collision_c.find("site").set(
"rgba", array_to_string([0, 1, 0, 1]))
else:
collision_c.find("site").set(
"rgba", array_to_string([0, 0, 1, 1]))
table_subtree.append(collision_c)
#Except the closest row to the robot
if i < self.num_squares[0] - 1 and place_this_sensor:
sensor_name = square_name2 + "_sensor"
sensor_site_name = square_name2 # +"_site"
self.sensor_names += [sensor_name]
self.sensor_site_names[sensor_name] = sensor_site_name
ET.SubElement(self.sensor,
"force",
attrib={
"name": sensor_name,
"site": sensor_site_name
})
#Add upper border to the table
table_subtree = self.worldbody.find(
".//body[@name='{}']".format("table"))
squares_height = 0.1
border_half_size = 0.08
square2 = BoxObject(size=[
border_half_size,
self.table_half_size[1] + 2 * border_half_size,
squares_height / 2 - 0.001
],
rgba=[0, 1, 0, 1],
density=1,
friction=0.05)
square_name2 = 'border_upper'
self.squares[square_name2] = square2
collision_c = square2.get_collision(name=square_name2, site=True)
collision_c.set(
"pos",
array_to_string([
self.table_half_size[0] + border_half_size, 0,
self.table_half_size[2] + squares_height / 2 - 0.001
]))
collision_c.find("site").set(
"pos", array_to_string([0, 0, squares_height / 2]))
collision_c.find("site").set(
"size",
array_to_string([
border_half_size,
self.table_half_size[1] + 2 * border_half_size, 0.002
]))
collision_c.find("site").set("rgba", array_to_string([0, 0, 1, 1]))
table_subtree.append(collision_c)
#Add left border to the table
square2 = BoxObject(size=[
self.table_half_size[0], border_half_size,
squares_height / 2 - 0.001
],
rgba=[0, 1, 0, 1],
density=1,
friction=0.05)
square_name2 = 'border_left'
self.squares[square_name2] = square2
collision_c = square2.get_collision(name=square_name2, site=True)
collision_c.set(
"pos",
array_to_string([
0, self.table_half_size[1] + border_half_size,
self.table_half_size[2] + squares_height / 2 - 0.001
]))
collision_c.find("site").set(
"pos", array_to_string([0, 0, squares_height / 2]))
collision_c.find("site").set(
"size",
array_to_string(
[self.table_half_size[0], border_half_size, 0.002]))
collision_c.find("site").set("rgba", array_to_string([0, 0, 1, 1]))
table_subtree.append(collision_c)
#Add right border to the table
square2 = BoxObject(size=[
self.table_half_size[0], border_half_size,
squares_height / 2 - 0.001
],
rgba=[0, 1, 0, 1],
density=1,
friction=0.05)
square_name2 = 'border_right'
self.squares[square_name2] = square2
collision_c = square2.get_collision(name=square_name2, site=True)
collision_c.set(
"pos",
array_to_string([
0, -self.table_half_size[1] - border_half_size,
self.table_half_size[2] + squares_height / 2 - 0.001
]))
collision_c.find("site").set(
"pos", array_to_string([0, 0, squares_height / 2]))
collision_c.find("site").set(
"size",
array_to_string(
[self.table_half_size[0], border_half_size, 0.002]))
collision_c.find("site").set("rgba", array_to_string([0, 0, 1, 1]))
table_subtree.append(collision_c)
#Add lower border to the table
low_border_half_size = 0.06
square2 = BoxObject(size=[
low_border_half_size,
self.table_half_size[1] + 2 * border_half_size,
squares_height / 2 - 0.001
],
rgba=[0, 1, 0, 1],
density=1,
friction=0.05)
square_name2 = 'border_lower'
self.squares[square_name2] = square2
collision_c = square2.get_collision(name=square_name2, site=True)
collision_c.set(
"pos",
array_to_string([
-self.table_half_size[0] - low_border_half_size, 0,
self.table_half_size[2] + squares_height / 2 - 0.001
]))
collision_c.find("site").set(
"pos", array_to_string([0, 0, squares_height / 2]))
collision_c.find("site").set(
"size",
array_to_string([
low_border_half_size,
self.table_half_size[1] + 2 * border_half_size, 0.002
]))
collision_c.find("site").set("rgba", array_to_string([0, 0, 1, 1]))
table_subtree.append(collision_c)
self.table_top.set(
"pos", array_to_string(np.array([0, 0, self.table_half_size[2]])))
def configure_location(self):
self.bottom_pos = np.array([0, 0, 0])
self.floor.set("pos", array_to_string(self.bottom_pos))
self.center_pos = self.bottom_pos + np.array(
[0, 0, 2 * self.table_half_size[2]])
self.table_body.set("pos", array_to_string(self.center_pos))
self.table_asset.set("size",
array_to_string(self.table_height_full_size))
self.table_body.set("friction", array_to_string(self.table_friction))
height_img = imageio.imread(xml_path_completion(
self.table_asset.get("file")),
as_gray=True)
total_pixels = height_img.shape[0] * height_img.shape[1]
#TODO: the number of sensors affects the performance. Do not increse over 500
#800 sensors -> mujoco steps up to 0.25sec per policy step!
num_sensors = min(500, total_pixels)
sample_rate = np.floor(total_pixels / num_sensors)
#Random Sampling:
used_pairs = []
sensor_counter = 0
while sensor_counter < num_sensors:
i = np.random.randint(0, high=height_img.shape[0])
j = np.random.randint(0, high=height_img.shape[1])
if (
i, j
) not in used_pairs: # We could add additional constraints like that sensors should be appart from each other
self.mujoco_objects = OrderedDict()
square = BallObject(size=[0.01],
rgba=[0, 0, 1, 1],
density=500,
friction=0.05)
square_name = 'contact_' + str(i) + "_" + str(j)
collision_b = square.get_collision(name=square_name, site=True)
x_pos = 0
if height_img.shape[0] % 2 == 0:
x_pos = -(np.floor(height_img.shape[0] / 2) - 0.5 -
i) * 2 * self.table_height_full_size[
0] / height_img.shape[0]
else:
x_pos = (i - np.floor(height_img.shape[0] / 2)
) * 2 * self.table_height_full_size[
0] / height_img.shape[0]
y_pos = 0
if height_img.shape[1] % 2 == 0:
y_pos = -(np.floor(height_img.shape[1] / 2) - 0.5 -
j) * 2 * self.table_height_full_size[
1] / height_img.shape[1]
else:
y_pos = (j - np.floor(height_img.shape[1] / 2)
) * 2 * self.table_height_full_size[
1] / height_img.shape[1]
#Compute mean of pixel value and neighbors -> better for low dim images
acc = 0
mean_ctr = 1
pixel_value = height_img[i, j]
# DO NOT DELETE! The following code helps for low dimensional pictures because the interpolation "buries" some sensors
# if i+1< height_img.shape[0]:
# if height_img[i+1,j] > pixel_value:
# acc += height_img[i+1,j]
# mean_ctr += 1
# if i-1<=0:
# if height_img[i-1,j] > pixel_value:
# acc += height_img[i-1,j]
# mean_ctr += 1
# if j+1< height_img.shape[0]:
# if height_img[i,j+1] > pixel_value:
# acc += height_img[i,j+1]
# mean_ctr += 1
# if j-1<=0:
# if height_img[i,j-1] > pixel_value:
# acc += height_img[i,j-1]
# mean_ctr += 1
acc += height_img[i, j]
z_pos = self.table_height_full_size[
3] + self.table_height_full_size[2] * (
acc / float(mean_ctr)) / np.max(height_img) + 0.005
position_sensor = [y_pos, x_pos, z_pos]
position_sensor[1] = -position_sensor[1]
collision_b.find("site").set("pos",
array_to_string(position_sensor))
self.worldbody.append(collision_b.find("site"))
self.sensor_sites[sensor_counter] = position_sensor
# Add this pixel to the list of used pixels
used_pairs += [(i, j)]
# Add sensor linked to the sensor site
ET.SubElement(self.sensor,
"touch",
attrib={
"name": square_name + "_sensor",
"site": square_name
}) # + "_site"})
sensor_counter += 1