def is_goal_reached(center_pos, segment_pos):
pole_pos = center_pos + POLE_POSITION_OFFSETS
n_enclosed = get_poles_enclosed(segment_pos, pole_pos)
if np.sum(n_enclosed) >= 3 and all_points_below_z(segment_pos,
max_z=GOAL_MAX_Z):
return True
return False
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 _is_goal_reached(self, segments_pos):
"""
Goal is reached if the centers of all three poles are contained in the dlo polygon and the segments
are below a certain height.
:return:
"""
n_enclosed = self._get_poles_enclosed(segments_pos)
if np.sum(n_enclosed) >= 3 and all_points_below_z(segments_pos,
max_z=GOAL_MAX_Z):
return True
return False
def compute_dense_reward_and_check_goal(center_pos, segment_pos_0,
segment_pos_1):
pole_pos = center_pos + POLE_POSITION_OFFSETS
poles_enclosed_0 = get_poles_enclosed(segment_pos_0, pole_pos)
poles_enclosed_1 = get_poles_enclosed(segment_pos_1, pole_pos)
poles_enclosed_diff = poles_enclosed_0 - poles_enclosed_1
# Check if final goal is reached
is_correct_height = all_points_below_z(segment_pos_0, max_z=GOAL_MAX_Z)
n_enclosed_0 = np.sum(poles_enclosed_0)
final_goal_reached = n_enclosed_0 >= 3 and is_correct_height
return np.sum(poles_enclosed_diff
) + 5 * float(final_goal_reached), final_goal_reached
def _compute_dense_reward_and_check_goal(self, segments_pos_0,
segments_pos_1):
"""
Compute reward for transition between two timesteps and check goal condition
:return:
"""
poles_enclosed_0 = self._get_poles_enclosed(segments_pos_0)
poles_enclosed_1 = self._get_poles_enclosed(segments_pos_1)
poles_enclosed_diff = poles_enclosed_0 - poles_enclosed_1
# Check if final goal is reached
is_correct_height = all_points_below_z(segments_pos_0,
max_z=GOAL_MAX_Z)
n_enclosed_0 = np.sum(poles_enclosed_0)
final_goal_reached = n_enclosed_0 >= 3 and is_correct_height
return np.sum(poles_enclosed_diff
) + 5 * float(final_goal_reached), final_goal_reached
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