def sample_pusher_position_and_velocity(T_aug=None):
low = np.array([-0.16, -0.08])
high = np.array([-0.15, 0.08])
q_pusher = random_sample_in_range(low, high)
q_pusher_3d_homog = np.array([0, 0, 0, 1.0])
q_pusher_3d_homog[:2] = q_pusher
# sample pusher velocity
vel_min = np.array([0.15])
vel_max = np.array([0.25])
magnitude = random_sample_in_range(vel_min, vel_max)
v_pusher_3d = magnitude * np.array([1, 0, 0])
if T_aug is not None:
v_pusher_3d = T_aug[:3, :3] @ v_pusher_3d
q_pusher_3d_homog = T_aug @ q_pusher_3d_homog
v_pusher = v_pusher_3d[:2]
q_pusher = q_pusher_3d_homog[:2]
return {'q_pusher': q_pusher,
'v_pusher': v_pusher,
}
def sample_pusher_velocity_3D():
vel_min = 0.15
vel_max = 0.25
magnitude = random_sample_in_range(vel_min, vel_max)
angle_max = np.deg2rad(30)
angle_min = -angle_max
angle = random_sample_in_range(angle_min, angle_max)
vel_3d = magnitude * np.array([np.cos(angle), np.sin(angle), 0])
return vel_3d
def sample_T_aug(pos_min, pos_max, yaw_min, yaw_max):
low = np.array(pos_min, dtype=np.float32)
high = np.array(pos_max, dtype=np.float32)
pos = random_sample_in_range(low, high)
yaw_min = np.array(yaw_min, dtype=np.float32)
yaw_max = np.array(yaw_max, dtype=np.float32)
yaw = random_sample_in_range(yaw_min, yaw_max)
T = np.eye(4)
T[:3, :3] = transforms3d.euler.euler2mat(0, 0, yaw)
T[:3, 3] = pos
return T
def sample_slider_position(T_aug=None):
# nominal position with box lying on it's side.
# the side with the recipe for "Colored Sugar" is facing upwards
pos = np.array([1.56907481e-04, 1.11390697e-06, 5.11972761e-02])
quat = np.array(
[7.13518047e-01, -6.69765583e-07, -7.00636851e-01, -6.82079212e-07])
T_O_slider = transform_utils.transform_from_pose(pos, quat)
# apply a random yaw to the object
yaw_min = 0
yaw_max = 2 * np.pi
yaw = random_sample_in_range(0, 2 * np.pi)
quat_yaw = transforms3d.euler.euler2quat(0, 0, yaw)
T_yaw = transform_utils.transform_from_pose([0, 0, 0], quat_yaw)
T_O_slider = np.matmul(T_yaw, T_O_slider)
T_W_slider = None
if T_aug is not None:
T_W_slider = T_aug @ T_O_slider
else:
T_W_slider = T_O_slider
pose_dict = transform_utils.matrix_to_dict(T_W_slider)
# note the quat/pos ordering
q = np.concatenate((pose_dict['quaternion'], pose_dict['position']))
return q
def sample_object_position(T_aug=None, upright=False):
pos = np.array([0, 0, 0.1])
quat = None
if upright:
quat = np.array([1, 0, 0, 0])
else:
quat = transforms3d.euler.euler2quat(np.deg2rad(90), 0, 0)
T_O_slider = transform_utils.transform_from_pose(pos, quat)
# apply a random yaw to the object
yaw_min = 0
yaw_max = 2 * np.pi
yaw = random_sample_in_range(0, 2 * np.pi)
quat_yaw = transforms3d.euler.euler2quat(0, 0, yaw)
T_yaw = transform_utils.transform_from_pose([0, 0, 0], quat_yaw)
T_O_slider = np.matmul(T_yaw, T_O_slider)
T_W_slider = None
if T_aug is not None:
T_W_slider = T_aug @ T_O_slider
else:
T_W_slider = T_O_slider
pose_dict = transform_utils.matrix_to_dict(T_W_slider)
# note the quat/pos ordering
q = np.concatenate((pose_dict['quaternion'], pose_dict['position']))
return q
def sample_slider_position(T_aug=None):
# always at the origin basically
pos = np.array([0, 0, 0.03])
yaw_min = np.array([0])
yaw_max = np.array([2 * np.pi])
yaw = random_sample_in_range(yaw_min, yaw_max)
quat = transforms3d.euler.euler2quat(0, 0, yaw)
T_O_slider = transform_utils.transform_from_pose(pos, quat)
T_W_slider = None
if T_aug is not None:
T_W_slider = T_aug @ T_O_slider
else:
T_W_slider = T_O_slider
pose_dict = transform_utils.matrix_to_dict(T_W_slider)
# note the quat/pos orderining
q = np.concatenate((pose_dict['quaternion'], pose_dict['position']))
return q
