def setup(self, *args, **kwargs):
super().setup(*args, **kwargs)
# the space only needs a sensor body
self.goal_body = pm.Body(body_type=pm.Body.STATIC)
self.goal_shape = pm.Poly.create_box(self.goal_body, (self.w, self.h))
self.goal_shape.sensor = True
self.goal_body.position = (self.x + self.w / 2, self.y - self.h / 2)
self.add_to_space(self.goal_body, self.goal_shape)
# Making visual display: region should consist of very lightly shaded
# rectangle, surrounded by darker stippled border. Ideally corners on
# the rectangle should be rounded.
self.rect_xform = r.Transform()
self.rect_xform.set_translation(*self.goal_body.position)
self.rect_xform.set_rotation(self.goal_body.angle)
inner_colour = lighten_rgb(self.base_colour, times=2)
inner_rect = r.make_rect(width=self.w, height=self.h, filled=True)
inner_rect.set_color(*inner_colour)
inner_rect.add_attr(self.rect_xform)
self.viewer.add_geom(inner_rect)
outer_colour = self.base_colour
outer_rect = r.make_rect(width=self.w, height=self.h, filled=False)
outer_rect.set_color(*outer_colour)
outer_rect.add_attr(r.LineStyle(0x00FF))
outer_rect.set_linewidth(250 * GOAL_LINE_THICKNESS)
outer_rect.add_attr(self.rect_xform)
self.viewer.add_geom(outer_rect)
def reset(self):
self._episode_steps = 0
# delete old entities/space
self._entities = []
self._space = None
self._robot = None
self._phys_vars = None
if self.viewer is None:
res_h, res_w = self.res_hw
background_colour = lighten_rgb(COLOURS_RGB['grey'], times=4)
self.viewer = r.Viewer(res_w,
res_h,
visible=False,
background_rgb=background_colour)
else:
# these will get added back later
self.viewer.reset_geoms()
self._space = pm.Space()
self._space.collision_slop = 0.01
self._space.iterations = self.phys_iter
if self.rand_dynamics:
# Randomise the physics properties of objects and the robot a
# little bit.
self._phys_vars = PhysicsVariables.sample(self.rng)
else:
self._phys_vars = PhysicsVariables.defaults()
# set up robot and arena
arena_l, arena_r, arena_b, arena_t = self.ARENA_BOUNDS_LRBT
self._arena = en.ArenaBoundaries(left=arena_l,
right=arena_r,
bottom=arena_b,
top=arena_t)
self._arena_w = arena_r - arena_l
self._arena_h = arena_t - arena_b
self.add_entities([self._arena])
reset_rv = self.on_reset()
assert reset_rv is None, \
f"on_reset method of {type(self)} returned {reset_rv}, but "\
f"should return None"
assert isinstance(self._robot, en.Robot)
assert len(self._entities) >= 1
assert np.allclose(self._arena.left + self._arena.right, 0)
assert np.allclose(self._arena.bottom + self._arena.top, 0)
self._use_allo_cam()
# # step forward by one second so PyMunk can recover from bad initial
# # conditions
# # (disabled some time in 2019; don't think it's necessary with
# # well-designed envs)
# forward_time = 1.0
# forward_frames = int(math.ceil(forward_time * self.fps))
# for _ in range(forward_frames):
# self._phys_steps_on_frame()
return self.render(mode='rgb_array')
def setup(self, *args, **kwargs):
super().setup(*args, **kwargs)
# physics setup, starting with main body
# signature: moment_for_circle(mass, inner_rad, outer_rad, offset)
inertia = pm.moment_for_circle(self.mass, 0, self.radius, (0, 0))
self.robot_body = body = pm.Body(self.mass, inertia)
body.position = self.init_pos
body.angle = self.init_angle
self.add_to_space(body)
# For control. The rough joint setup was taken form tank.py in the
# pymunk examples.
self.control_body = control_body = pm.Body(body_type=pm.Body.KINEMATIC)
control_body.position = self.init_pos
control_body.angle = self.init_angle
self.add_to_space(control_body)
pos_control_joint = pm.PivotJoint(control_body, body, (0, 0), (0, 0))
pos_control_joint.max_bias = 0
pos_control_joint.max_force = self.phys_vars.robot_pos_joint_max_force
self.add_to_space(pos_control_joint)
rot_control_joint = pm.GearJoint(control_body, body, 0.0, 1.0)
rot_control_joint.error_bias = 0.0
rot_control_joint.max_bias = 2.5
rot_control_joint.max_force = self.phys_vars.robot_rot_joint_max_force
self.add_to_space(rot_control_joint)
# googly eye control bodies & joints
self.pupil_bodies = []
for eye_side in [-1, 1]:
eye_mass = self.mass / 10
eye_radius = self.radius
eye_inertia = pm.moment_for_circle(eye_mass, 0, eye_radius, (0, 0))
eye_body = pm.Body(eye_mass, eye_inertia)
eye_body.angle = self.init_angle
eye_joint = pm.DampedRotarySpring(body, eye_body, 0, 0.1, 3e-3)
eye_joint.max_bias = 3.0
eye_joint.max_force = 0.001
self.pupil_bodies.append(eye_body)
self.add_to_space(eye_body, eye_joint)
# finger bodies/controls (annoying)
finger_thickness = 0.25 * self.radius
finger_upper_length = 1.1 * self.radius
finger_lower_length = 0.7 * self.radius
self.finger_bodies = []
self.finger_motors = []
finger_vertices = []
finger_inner_vertices = []
self.target_finger_angle = 0.0
for finger_side in [-1, 1]:
# basic finger body
finger_verts = make_finger_vertices(
upper_arm_len=finger_upper_length,
forearm_len=finger_lower_length,
thickness=finger_thickness,
side_sign=finger_side)
finger_vertices.append(finger_verts)
finger_inner_verts = make_finger_vertices(
upper_arm_len=finger_upper_length - ROBOT_LINE_THICKNESS * 2,
forearm_len=finger_lower_length - ROBOT_LINE_THICKNESS * 2,
thickness=finger_thickness - ROBOT_LINE_THICKNESS * 2,
side_sign=finger_side)
finger_inner_verts = [[(x, y + ROBOT_LINE_THICKNESS)
for x, y in box]
for box in finger_inner_verts]
finger_inner_vertices.append(finger_inner_verts)
# these are movement limits; they are useful below, but also
# necessary to make initial positioning work
if finger_side < 0:
lower_rot_lim = -self.finger_rot_limit_inner
upper_rot_lim = self.finger_rot_limit_outer
if finger_side > 0:
lower_rot_lim = -self.finger_rot_limit_outer
upper_rot_lim = self.finger_rot_limit_inner
finger_mass = self.mass / 8
finger_inertia = pm.moment_for_poly(finger_mass,
sum(finger_verts, []))
finger_body = pm.Body(finger_mass, finger_inertia)
if finger_side < 0:
delta_finger_angle = upper_rot_lim
finger_body.angle = self.init_angle + delta_finger_angle
else:
delta_finger_angle = lower_rot_lim
finger_body.angle = self.init_angle + delta_finger_angle
# position of finger relative to body
finger_rel_pos = (finger_side * self.radius * 0.45,
self.radius * 0.1)
finger_rel_pos_rot = gtools.rotate_vec(finger_rel_pos,
self.init_angle)
finger_body.position = gtools.add_vecs(body.position,
finger_rel_pos_rot)
self.add_to_space(finger_body)
self.finger_bodies.append(finger_body)
# pin joint to keep it in place (it will rotate around this point)
finger_pin = pm.PinJoint(
body,
finger_body,
finger_rel_pos,
(0, 0),
)
finger_pin.error_bias = 0.0
self.add_to_space(finger_pin)
# rotary limit joint to stop it from getting too far out of line
finger_limit = pm.RotaryLimitJoint(body, finger_body,
lower_rot_lim, upper_rot_lim)
finger_limit.error_bias = 0.0
self.add_to_space(finger_limit)
# motor to move the fingers around (very limited in power so as not
# to conflict with rotary limit joint)
finger_motor = pm.SimpleMotor(body, finger_body, 0.0)
finger_motor.rate = 0.0
finger_motor.max_bias = 0.0
finger_motor.max_force = self.phys_vars.robot_finger_max_force
self.add_to_space(finger_motor)
self.finger_motors.append(finger_motor)
# For collision. Main body circle. Signature: Circle(body, radius,
# offset).
robot_group = 1
body_shape = pm.Circle(body, self.radius, (0, 0))
body_shape.filter = pm.ShapeFilter(group=robot_group)
body_shape.friction = 0.5
self.add_to_space(body_shape)
# the fingers
finger_shapes = []
for finger_body, finger_verts, finger_side in zip(
self.finger_bodies, finger_vertices, [-1, 1]):
finger_subshapes = []
for finger_subverts in finger_verts:
finger_subshape = pm.Poly(finger_body, finger_subverts)
finger_subshape.filter = pm.ShapeFilter(group=robot_group)
# grippy fingers
finger_subshape.friction = 5.0
finger_subshapes.append(finger_subshape)
self.add_to_space(*finger_subshapes)
finger_shapes.append(finger_subshapes)
# graphics setup
# draw a circular body
circ_body_in = r.make_circle(radius=self.radius - ROBOT_LINE_THICKNESS,
res=100)
circ_body_out = r.make_circle(radius=self.radius, res=100)
robot_colour = COLOURS_RGB['grey']
dark_robot_colour = darken_rgb(robot_colour)
light_robot_colour = lighten_rgb(robot_colour, 4)
circ_body_in.set_color(*robot_colour)
circ_body_out.set_color(*dark_robot_colour)
# draw the two fingers
self.finger_xforms = []
finger_outer_geoms = []
finger_inner_geoms = []
for finger_outer_subshapes, finger_inner_verts, finger_side in zip(
finger_shapes, finger_inner_vertices, [-1, 1]):
finger_xform = r.Transform()
self.finger_xforms.append(finger_xform)
for finger_subshape in finger_outer_subshapes:
vertices = [(v.x, v.y) for v in finger_subshape.get_vertices()]
finger_outer_geom = r.make_polygon(vertices)
finger_outer_geom.set_color(*robot_colour)
finger_outer_geom.add_attr(finger_xform)
finger_outer_geoms.append(finger_outer_geom)
for vertices in finger_inner_verts:
finger_inner_geom = r.make_polygon(vertices)
finger_inner_geom.set_color(*light_robot_colour)
finger_inner_geom.add_attr(finger_xform)
finger_inner_geoms.append(finger_inner_geom)
for geom in finger_outer_geoms:
self.viewer.add_geom(geom)
for geom in finger_inner_geoms:
self.viewer.add_geom(geom)
# draw some cute eyes
eye_shapes = []
self.pupil_transforms = []
for x_sign in [-1, 1]:
eye = r.make_circle(radius=0.2 * self.radius, res=20)
eye.set_color(1.0, 1.0, 1.0)
eye_base_transform = r.Transform().set_translation(
x_sign * 0.4 * self.radius, 0.3 * self.radius)
eye.add_attr(eye_base_transform)
pupil = r.make_circle(radius=0.12 * self.radius, res=10)
pupil.set_color(0.1, 0.1, 0.1)
# The pupils point forward slightly
pupil_transform = r.Transform()
pupil.add_attr(r.Transform().set_translation(
0, self.radius * 0.07))
pupil.add_attr(pupil_transform)
pupil.add_attr(eye_base_transform)
self.pupil_transforms.append(pupil_transform)
eye_shapes.extend([eye, pupil])
# join them together
self.robot_xform = r.Transform()
robot_compound = r.Compound([circ_body_out, circ_body_in, *eye_shapes])
robot_compound.add_attr(self.robot_xform)
self.viewer.add_geom(robot_compound)