def _compute_dense_reward_and_check_goal(self, segments_pos_0,
segments_pos_1):
# Get position of goal
goal_pos = self.sim.getRigidBody("obstacle_goal").getPosition()
pole_enclosed_0 = point_inside_polygon(
np.array(segments_pos_0)[:, 0:2], goal_pos)
pole_enclosed_1 = point_inside_polygon(
np.array(segments_pos_1)[:, 0:2], goal_pos)
poles_enclosed_diff = pole_enclosed_0 - pole_enclosed_1
# Check if final goal is reached
is_correct_height = all_points_below_z(segments_pos_0,
max_z=GOAL_MAX_Z)
# Check if grippers are close enough to each other
position_g0 = to_numpy_array(
self.sim.getRigidBody("gripper_0").getPosition())
position_g1 = to_numpy_array(
self.sim.getRigidBody("gripper_1").getPosition())
is_grippers_close = np.linalg.norm(position_g1 - position_g0) < 0.02
final_goal_reached = pole_enclosed_0 and is_correct_height and is_grippers_close
return poles_enclosed_diff + 5 * float(
final_goal_reached), final_goal_reached
def _is_goal_reached(self, segment_pos):
"""
Goal is reached if the cable is closed around the center obstacle. This is the case if the segments
are on the ground, the grippers are close to each other and the center pole is within the
dlo polygon.
:return:
"""
# Get position of goal
goal_pos = self.sim.getRigidBody("obstacle_goal").getPosition()
# Check if goal obstacle in enclosed by dlo
is_within_polygon = point_inside_polygon(
np.array(segment_pos)[:, 0:2], goal_pos)
# Check if cable has correct height
is_correct_height = all_points_below_z(segment_pos, max_z=GOAL_MAX_Z)
# Check if grippers are close enough to each other
position_g0 = to_numpy_array(
self.sim.getRigidBody("gripper_0").getPosition())
position_g1 = to_numpy_array(
self.sim.getRigidBody("gripper_1").getPosition())
is_grippers_close = np.linalg.norm(position_g1 - position_g0) < 0.01
if is_within_polygon and is_correct_height and is_grippers_close:
return True
return False
def get_poles_enclosed(segments_pos, pole_pos):
poles_enclosed = np.zeros(3)
segments_xy = np.array(segments_pos)[:, 0:2]
for i in range(0, 3):
is_within_polygon = point_inside_polygon(segments_xy, pole_pos[i])
poles_enclosed[i] = int(is_within_polygon)
return poles_enclosed
def _get_poles_enclosed(self, segments_pos):
"""
Check how many poles the rubber band encloses
:param segments_pos:
:return:
"""
poles_enclosed = np.zeros(3)
ground_pos = self.sim.getRigidBody("ground").getPosition()
pole_pos = np.array([ground_pos[0], ground_pos[1]]) + POLE_OFFSET
for i in range(0, 3):
segments_xy = np.array(segments_pos)[:, 0:2]
is_within_polygon = point_inside_polygon(segments_xy, pole_pos[i])
poles_enclosed[i] = int(is_within_polygon)
return poles_enclosed
def is_goal_reached(sim, segments_pos):
# Get position of goal
goal_pos = sim.getRigidBody("obstacle_goal").getPosition()
# Check if goal obstacle in enclosed by dlo
is_within_polygon = point_inside_polygon(np.array(segments_pos)[:,0:2], goal_pos)
# Check if cable has correct height
is_correct_height = all_points_below_z(segments_pos, max_z=GOAL_MAX_Z)
# Check if grippers are close enough to each other
position_g0 = to_numpy_array(sim.getRigidBody("gripper_0").getPosition())
position_g1 = to_numpy_array(sim.getRigidBody("gripper_1").getPosition())
is_grippers_close = np.linalg.norm(position_g1-position_g0) < 0.02
if is_within_polygon and is_correct_height and is_grippers_close:
return True
return False