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(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
def get_rigid_body_velocity(sim, name):
"""Get velocity of AGX rigid body.
:param sim: AGX Dynamics simulation object
:param name: name of rigid body
:return: NumPy array with rigid body rotation
"""
rigid_body = sim.getRigidBody(name)
velocity = to_numpy_array(rigid_body.getVelocity())
logger.debug("Rigid body {}, velocity: {}".format(name, velocity))
return velocity
def get_rigid_body_rotation(sim, name):
"""Get rotation of AGX rigid body.
:param sim: AGX Dynamics simulation object
:param name: name of rigid body
:return: NumPy array with rigid body rotation
"""
rigid_body = sim.getRigidBody(name)
rotation = to_numpy_array(rigid_body.getRotation())
logger.debug("Rigid body {}, rotation: {}".format(name, rotation))
return rotation
def get_rigid_body_position(sim, key):
"""Get position of AGX rigid body.
:param sim: AGX Dynamics simulation object
:param key: name of rigid body
:return: NumPy array with rigid body position
"""
rigid_body = sim.getRigidBody(key)
position = to_numpy_array(rigid_body.getPosition())
logger.debug("Rigid body {}, position: {}".format(key, position))
return position
def _compute_segments_pos(self):
segments_pos = []
dlo = agxCable.Cable.find(self.sim, "DLO")
segment_iterator = dlo.begin()
n_segments = dlo.getNumSegments()
for i in range(n_segments):
if not segment_iterator.isEnd():
pos = segment_iterator.getGeometry().getPosition()
segments_pos.append(to_numpy_array(pos))
segment_iterator.inc()
return segments_pos
def get_constraint_force_torque(sim, name, constraint_name):
"""Gets force an torque on rigid object, computed by a constraint defined by 'constraint_name'.
:param sim: AGX Simulation object
:param name: name of rigid body
:param constraint_name: Name indicating which constraint contains force torque information for this object
:return: force an torque
"""
rigid_body = sim.getRigidBody(name)
constraint = sim.getConstraint(constraint_name)
force = agx.Vec3()
torque = agx.Vec3()
constraint.getLastForce(rigid_body, force, torque)
force = to_numpy_array(force)
torque = to_numpy_array(torque)
logger.debug("Constraint {}, force: {}, torque: {}".format(
constraint_name, force, torque))
return np.concatenate((force, torque))
def get_cable_segment_positions_and_velocities(cable):
"""Get AGX Cable segments' center of mass positions.
:param cable: AGX Cable object
:return: NumPy array with segments' positions
"""
num_segments = cable.getNumSegments()
cable_positions = np.zeros(shape=(3, num_segments))
cable_velocities = np.zeros(shape=(3, num_segments))
segment_iterator = cable.begin()
for i in range(num_segments):
if not segment_iterator.isEnd():
position = segment_iterator.getRigidBody().getPosition()
velocity = segment_iterator.getRigidBody().getVelocity()
cable_positions[:, i] = to_numpy_array(position)
cable_velocities[:, i] = to_numpy_array(velocity)
segment_iterator.inc()
else:
logger.error(
'AGX segment iteration finished early. Number or cable segments may be wrong.'
)
return cable_positions, cable_velocities
def get_ring_segment_positions(sim, ring_name, num_segments=None):
"""Get ring segments positions.
:param sim: AGX Dynamics simulation object
:param ring_name: name of ring object
:param num_segments: number of segments making up the ring (possibly saves search time)
:return: NumPy array with segments' positions
"""
if not num_segments:
rbs = sim.getRigidBodies()
ring_segments_positions = np.zeros(shape=(3, ))
for rb in rbs:
if ring_name in rb.getName():
position = to_numpy_array(rb.getPosition())
ring_segments_positions = np.stack(
[ring_segments_positions, position], axis=0)
else:
ring_segments_positions = np.zeros(shape=(3, num_segments + 1))
for i in range(1, num_segments + 1):
rb = sim.getRigidBody(ring_name + "_" + str(i))
ring_segments_positions[:,
i - 1] = to_numpy_array(rb.getPosition())
return ring_segments_positions
def sample_random_goal(sim, app=None, dof_vector=np.ones(3)):
"""Goal Randomization: Sample 2 points, and execute point-to-point trajectory
:param sim: AGX Dynamics simulation object
:param app: AGX Dynamics application object
:param np.array dof_vector: desired degrees of freedom of the gripper(s), [x, y, z]
"""
assert np.sum(dof_vector) >= 1, "There must be at least one degree of freedom"
right_motor_x = sim.getConstraint1DOF("gripper_right_goal_joint_base_x").getMotor1D()
right_motor_y = sim.getConstraint1DOF("gripper_right_goal_joint_base_y").getMotor1D()
right_motor_z = sim.getConstraint1DOF("gripper_right_goal_joint_base_z").getMotor1D()
right_gripper = sim.getRigidBody("gripper_right_goal")
settling_time = 1
n_waypoints = 2
n_seconds = 10*n_waypoints + settling_time
center = np.array([2 * (2 * RADIUS + SIZE_GRIPPER), 0, 0])
waypoints = [to_numpy_array(right_gripper.getPosition())]
scales = np.zeros(n_waypoints)
for i in range(0, n_waypoints):
waypoint, length = sample_sphere(center, [2 * (2 * RADIUS + SIZE_GRIPPER) + 4 * SIZE_GRIPPER,
LENGTH - 4 * SIZE_GRIPPER], [0, np.pi], [-np.pi / 2, np.pi / 2])
waypoints.append(waypoint)
scales[i] = length
norm_scales = scales / sum(scales)
time_scales = norm_scales*n_seconds
t = sim.getTimeStamp()
start_time = t
while t < n_seconds:
if app:
app.executeOneStepWithGraphics()
velocity = polynomial_trajectory(t, start_time, waypoints, time_scales, degree=3)
velocity = velocity*dof_vector
right_motor_x.setSpeed(velocity[0])
right_motor_y.setSpeed(velocity[1])
right_motor_z.setSpeed(velocity[2])
t = sim.getTimeStamp()
t_0 = t
while t < t_0 + TIMESTEP * N_SUBSTEPS:
sim.stepForward()
t = sim.getTimeStamp()
# reset timestamp, after simulation
sim.setTimeStamp(0)
def get_cable_segment_rotations(cable):
"""Get AGX Cable segments' center of mass rotations.
:param cable: AGX Cable object
:return: NumPy array with segments' rotations
"""
num_segments = cable.getNumSegments()
cable_segments_rotations = np.zeros(shape=(4, num_segments))
segment_iterator = cable.begin()
for i in range(num_segments):
if not segment_iterator.isEnd():
rotation = segment_iterator.getGeometry().getRotation()
cable_segments_rotations[:, i] = to_numpy_array(rotation)
segment_iterator.inc()
else:
logger.error(
'AGX segment iteration finished early. Number or cable segments may be wrong.'
)
return cable_segments_rotations
def _get_observation(self):
rgb_buffer = None
depth_buffer = None
for buffer in self.render_to_image:
name = buffer.getName()
if name == 'rgb_buffer':
rgb_buffer = buffer
elif name == 'depth_buffer':
depth_buffer = buffer
assert self.observation_type in ("rgb", "depth", "rgb_and_depth",
"pos", "pos_and_vel")
if self.observation_type == "rgb":
image_ptr = rgb_buffer.getImageData()
image_data = create_numpy_array(
image_ptr, (self.image_size[0], self.image_size[1], 3),
np.uint8)
obs = np.flipud(image_data)
elif self.observation_type == "depth":
image_ptr = depth_buffer.getImageData()
image_data = create_numpy_array(
image_ptr, (self.image_size[0], self.image_size[1]),
np.float32)
obs = np.flipud(image_data)
elif self.observation_type == "rgb_and_depth":
obs = np.zeros((self.image_size[0], self.image_size[1], 4),
dtype=np.float32)
image_ptr = rgb_buffer.getImageData()
image_data = create_numpy_array(
image_ptr, (self.image_size[0], self.image_size[1], 3),
np.uint8)
obs[:, :, 0:3] = np.flipud(image_data.astype(np.float32)) / 255
image_ptr = depth_buffer.getImageData()
image_data = create_numpy_array(
image_ptr, (self.image_size[0], self.image_size[1]),
np.float32)
obs[:, :, 3] = np.flipud(image_data)
elif self.observation_type == "pos":
seg_pos = get_cable_segment_positions(
cable=agxCable.Cable.find(self.sim, "DLO")).flatten()
gripper = self.sim.getRigidBody("gripper_body")
gripper_pos = to_numpy_array(gripper.getPosition())[0:3]
cylinder = self.sim.getRigidBody("hollow_cylinder")
cylinder_pos = cylinder.getPosition()
ea = agx.EulerAngles().set(gripper.getRotation())
gripper_rot = ea.y()
obs = np.concatenate([
gripper_pos, [gripper_rot], seg_pos,
[cylinder_pos[0], cylinder_pos[1]]
])
elif self.observation_type == "pos_and_vel":
seg_pos, seg_vel = get_cable_segment_positions_and_velocities(
cable=agxCable.Cable.find(self.sim, "DLO"))
seg_pos = seg_pos.flatten()
seg_vel = seg_vel.flatten()
gripper = self.sim.getRigidBody("gripper_body")
gripper_pos = to_numpy_array(gripper.getPosition())[0:3]
gripper_vel = to_numpy_array(gripper.getVelocity())[0:3]
gripper_vel_rot = to_numpy_array(gripper.getAngularVelocity())[2]
cylinder = self.sim.getRigidBody("hollow_cylinder")
cylinder_pos = cylinder.getPosition()
ea = agx.EulerAngles().set(gripper.getRotation())
gripper_rot = ea.y()
obs = np.concatenate([
gripper_pos, [gripper_rot], gripper_vel, [gripper_vel_rot],
seg_pos, seg_vel, [cylinder_pos[0], cylinder_pos[1]]
])
return obs
def _get_observation(self):
rgb_buffer = None
depth_buffer = None
for buffer in self.render_to_image:
name = buffer.getName()
if name == 'rgb_buffer':
rgb_buffer = buffer
elif name == 'depth_buffer':
depth_buffer = buffer
assert self.observation_type in ("rgb", "depth", "rgb_and_depth",
"pos", "pos_and_vel")
if self.observation_type == "rgb":
image_ptr = rgb_buffer.getImageData()
image_data = create_numpy_array(
image_ptr, (self.image_size[0], self.image_size[1], 3),
np.uint8)
obs = np.flipud(image_data)
elif self.observation_type == "depth":
image_ptr = depth_buffer.getImageData()
image_data = create_numpy_array(
image_ptr, (self.image_size[0], self.image_size[1]),
np.float32)
obs = np.flipud(image_data)
elif self.observation_type == "rgb_and_depth":
obs = np.zeros((self.image_size[0], self.image_size[1], 4),
dtype=np.float32)
image_ptr = rgb_buffer.getImageData()
image_data = create_numpy_array(
image_ptr, (self.image_size[0], self.image_size[1], 3),
np.uint8)
obs[:, :, 0:3] = np.flipud(image_data.astype(np.float32)) / 255
image_ptr = depth_buffer.getImageData()
image_data = create_numpy_array(
image_ptr, (self.image_size[0], self.image_size[1]),
np.float32)
obs[:, :, 3] = np.flipud(image_data)
elif self.observation_type == "pos":
goal_pos = to_numpy_array(
self.sim.getRigidBody("ground").getPosition())[0:2]
seg_pos = get_cable_segment_positions(
cable=agxCable.Cable.find(self.sim, "DLO")).flatten()
gripper = self.sim.getRigidBody("gripper")
gripper_pos = to_numpy_array(gripper.getPosition())[0:3]
obs = np.concatenate([gripper_pos, seg_pos, goal_pos])
elif self.observation_type == "pos_and_vel":
goal_pos = to_numpy_array(
self.sim.getRigidBody("ground").getPosition())[0:2]
seg_pos, seg_vel = get_cable_segment_positions_and_velocities(
cable=agxCable.Cable.find(self.sim, "DLO"))
seg_pos = seg_pos.flatten()
seg_vel = seg_vel.flatten()
gripper = self.sim.getRigidBody("gripper")
gripper_pos = to_numpy_array(gripper.getPosition())[0:3]
gripper_vel = to_numpy_array(gripper.getVelocity())[0:3]
obs = np.concatenate(
[gripper_pos, gripper_vel, seg_pos, seg_vel, goal_pos])
return obs
def sample_random_goal(sim, app=None, dof_vector=np.ones(3)):
"""Goal Randomization: Sample 2 points, and execute point-to-point trajectory
:param sim: AGX Dynamics simulation object
:param app: AGX Dynamics application object
:param np.array dof_vector: desired degrees of freedom of the gripper(s), [x, y, z]
"""
assert np.sum(
dof_vector) >= 1, "There must be at least one degree of freedom"
right_gripper = sim.getRigidBody("gripper_right_goal")
right_motor_x = sim.getConstraint1DOF(
"gripper_right_goal_joint_base_x").getMotor1D()
right_motor_y = sim.getConstraint1DOF(
"gripper_right_goal_joint_base_y").getMotor1D()
right_motor_z = sim.getConstraint1DOF(
"gripper_right_goal_joint_base_z").getMotor1D()
left_gripper = sim.getRigidBody("gripper_left_goal")
left_motor_x = sim.getConstraint1DOF(
"gripper_left_goal_joint_base_x").getMotor1D()
left_motor_y = sim.getConstraint1DOF(
"gripper_left_goal_joint_base_y").getMotor1D()
left_motor_z = sim.getConstraint1DOF(
"gripper_left_goal_joint_base_z").getMotor1D()
settling_time = 1
n_seconds = 10 + settling_time
center = np.array([0, 0, -CYLINDER_RADIUS])
# Define trajectory waypoints
waypoints_right = [
to_numpy_array(right_gripper.getPosition()),
np.array([LENGTH / 2, 0, -CYLINDER_RADIUS])
]
scales_right = np.array([CYLINDER_RADIUS, 0])
goal_point_right, scales_right[1] = sample_sphere(
center, [2 * CYLINDER_RADIUS, LENGTH / 2], [np.pi / 2, np.pi], [
-math.asin(CYLINDER_LENGTH / LENGTH),
math.asin(CYLINDER_LENGTH / LENGTH)
])
goal_point_right[0] = min(goal_point_right[0], CYLINDER_RADIUS)
waypoints_right.append(goal_point_right)
norm_scales_right = scales_right / sum(scales_right)
time_scales_right = norm_scales_right * n_seconds
scales_left = np.array([CYLINDER_RADIUS, 0])
waypoints_left = [
to_numpy_array(left_gripper.getPosition()),
np.array([-LENGTH / 2, 0, -CYLINDER_RADIUS])
]
goal_point_left, scales_left[1] = sample_sphere(
center, [2 * CYLINDER_RADIUS, LENGTH / 2], [np.pi, 3 * np.pi / 2],
[0, 0])
goal_point_left[0] = max(goal_point_left[0], -CYLINDER_RADIUS)
waypoints_left.append(goal_point_left)
norm_scales_left = scales_left / sum(scales_left)
time_scales_left = norm_scales_left * n_seconds
t = sim.getTimeStamp()
start_time = t
while t < n_seconds:
if app:
app.executeOneStepWithGraphics()
right_velocity = polynomial_trajectory(t,
start_time,
waypoints_right,
time_scales_right,
degree=3)
right_velocity = right_velocity * dof_vector
right_motor_x.setSpeed(right_velocity[0])
right_motor_y.setSpeed(right_velocity[1])
right_motor_z.setSpeed(right_velocity[2])
left_velocity = polynomial_trajectory(t,
start_time,
waypoints_left,
time_scales_left,
degree=3)
left_velocity = left_velocity * dof_vector
left_motor_x.setSpeed(left_velocity[0])
left_motor_y.setSpeed(left_velocity[1])
left_motor_z.setSpeed(left_velocity[2])
t = sim.getTimeStamp()
t_0 = t
while t < t_0 + TIMESTEP * N_SUBSTEPS:
sim.stepForward()
t = sim.getTimeStamp()
# reset timestamp, after simulation
sim.setTimeStamp(0)
def sample_fixed_goal(sim, app=None):
"""Define the trajectory to generate fixed goal
:param sim: AGX Dynamics simulation object
:param app: AGX Dynamics application object
"""
right_gripper = sim.getRigidBody("gripper_right_goal")
right_motor_x = sim.getConstraint1DOF(
"gripper_right_goal_joint_base_x").getMotor1D()
right_motor_y = sim.getConstraint1DOF(
"gripper_right_goal_joint_base_y").getMotor1D()
right_motor_z = sim.getConstraint1DOF(
"gripper_right_goal_joint_base_z").getMotor1D()
left_gripper = sim.getRigidBody("gripper_left_goal")
left_motor_x = sim.getConstraint1DOF(
"gripper_left_goal_joint_base_x").getMotor1D()
left_motor_y = sim.getConstraint1DOF(
"gripper_left_goal_joint_base_y").getMotor1D()
left_motor_z = sim.getConstraint1DOF(
"gripper_left_goal_joint_base_z").getMotor1D()
# Define trajectory waypoints
waypoints_right = [
to_numpy_array(right_gripper.getPosition()),
np.array([LENGTH / 2, 0, -LENGTH / 2])
]
waypoints_left = [
to_numpy_array(left_gripper.getPosition()),
np.array([-CYLINDER_RADIUS, 0, -LENGTH / 2])
]
n_seconds = 20
time_scale = np.array([n_seconds - 1])
t = sim.getTimeStamp()
start_time = t
while t < n_seconds:
if app:
app.executeOneStepWithGraphics()
velocity_right = polynomial_trajectory(t,
start_time,
waypoints_right,
time_scale,
degree=3)
right_motor_x.setSpeed(velocity_right[0])
right_motor_y.setSpeed(velocity_right[1])
right_motor_z.setSpeed(velocity_right[2])
velocity_left = polynomial_trajectory(t,
start_time,
waypoints_left,
time_scale,
degree=3)
left_motor_x.setSpeed(velocity_left[0])
left_motor_y.setSpeed(velocity_left[1])
left_motor_z.setSpeed(velocity_left[2])
t = sim.getTimeStamp()
t_0 = t
while t < t_0 + TIMESTEP * N_SUBSTEPS:
sim.stepForward()
t = sim.getTimeStamp()