59 lines
1.8 KiB
Python
59 lines
1.8 KiB
Python
import numpy as np
|
|
|
|
|
|
def sample_insertion_pose():
|
|
# Peg
|
|
x_range = [0.1, 0.2]
|
|
y_range = [0.4, 0.6]
|
|
z_range = [0.05, 0.05]
|
|
|
|
ranges = np.vstack([x_range, y_range, z_range])
|
|
peg_position = np.random.uniform(ranges[:, 0], ranges[:, 1])
|
|
|
|
peg_quat = np.array([1, 0, 0, 0])
|
|
peg_pose = np.concatenate([peg_position, peg_quat])
|
|
|
|
# Socket
|
|
x_range = [-0.2, -0.1]
|
|
y_range = [0.4, 0.6]
|
|
z_range = [0.05, 0.05]
|
|
|
|
ranges = np.vstack([x_range, y_range, z_range])
|
|
socket_position = np.random.uniform(ranges[:, 0], ranges[:, 1])
|
|
|
|
socket_quat = np.array([1, 0, 0, 0])
|
|
socket_pose = np.concatenate([socket_position, socket_quat])
|
|
|
|
return peg_pose, socket_pose
|
|
|
|
def sample_transfer_pose():
|
|
# Box
|
|
x_range = [-0.2, 0.2]
|
|
y_range = [0.7, 1.1]
|
|
z_range = [0.47, 0.47]
|
|
|
|
ranges = np.vstack([x_range, y_range, z_range])
|
|
box_position = np.random.uniform(ranges[:, 0], ranges[:, 1])
|
|
|
|
|
|
return box_position
|
|
|
|
|
|
def sample_air_insert_socket_peg_state():
|
|
socket_position = np.random.uniform(
|
|
low=np.array([-0.20, 0.80, 0.472], dtype=np.float32),
|
|
high=np.array([-0.10, 1.00, 0.472], dtype=np.float32),
|
|
)
|
|
peg_position = np.random.uniform(
|
|
low=np.array([0.10, 0.80, 0.46], dtype=np.float32),
|
|
high=np.array([0.20, 1.00, 0.46], dtype=np.float32),
|
|
)
|
|
socket_quat = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32)
|
|
peg_quat = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32)
|
|
return {
|
|
"socket_pos": socket_position.astype(np.float32, copy=False),
|
|
"socket_quat": socket_quat,
|
|
"peg_pos": peg_position.astype(np.float32, copy=False),
|
|
"peg_quat": peg_quat,
|
|
}
|