def __init__(self,
env,
pos,
quat,
slacky_collision=True,
halfsize=VIRTUAL_CUBOID_HALF_SIZE,
ignore_collision=False,
avoid_edge_faces=True,
yawing_grasp=False,
allow_partial_sol=False):
self.object_pos = pos
self.object_ori = quat
self.ik = env.pinocchio_utils.inverse_kinematics
self.id = env.platform.simfinger.finger_id
self.tip_ids = env.platform.simfinger.pybullet_tip_link_indices
self.link_ids = env.platform.simfinger.pybullet_link_indices
self.T_cube_to_base = Transform(pos, quat)
self.T_base_to_cube = self.T_cube_to_base.inverse()
self.env = env
self.ik_utils = IKUtils(env, yawing_grasp=yawing_grasp)
self.slacky_collision = slacky_collision
self._org_tips_init = np.array(
self.env.platform.forward_kinematics(INIT_JOINT_CONF))
self.halfsize = halfsize
self.tip_solver = CuboidForceClosureTest(halfsize, CoulombFriction(MU))
self.ignore_collision = ignore_collision
self.avoid_edge_faces = avoid_edge_faces
self.yawing_grasp = yawing_grasp
self.allow_partial_sol = allow_partial_sol
def get_anticlockwise_two_finger_yawing_grasp(half_size, object_ori):
R_base_to_cube = Transform(np.zeros(3), object_ori).inverse()
z_cube = R_base_to_cube(np.array([0, 0, 1]))
axis = np.argmax(np.abs(z_cube))
above_point = []
for i in range(3):
if i == axis:
above_point.append(half_size[i] + 0.02)
else:
above_point.append(0)
side_centers = get_side_face_centers(half_size, object_ori)
tip_to_center = 0.20
ax1 = side_centers[1] - side_centers[0]
ax2 = side_centers[3] - side_centers[2]
g1 = np.array([
side_centers[0] - tip_to_center * ax2,
side_centers[1] + tip_to_center * ax2, above_point
])
g2 = np.array([
side_centers[0] + tip_to_center * ax2,
side_centers[1] - tip_to_center * ax2, above_point
])
# return [g1, g2]
# return [g2]
point = np.stack([
#[0, half_size[1], 0],
#[0, -half_size[1], 0],
side_centers[0],
side_centers[1],
above_point
])
return [point]
def _get_tip_path(cube_tip_positions, cube_path):
def get_quat(euler):
return p.getQuaternionFromEuler(euler)
return [
Transform(cube_pose[:3], get_quat(cube_pose[3:]))(cube_tip_positions)
for cube_pose in cube_path
]
def sample_side_face(n, half_size, object_ori, shrink_region=[0.0, 0.6, 0.0]):
R_base_to_cube = Transform(np.zeros(3), object_ori).inverse()
z_cube = R_base_to_cube(np.array([0, 0, 1]))
axis = np.argmax(np.abs(z_cube))
sample_ax = np.array([i for i in range(3) if i != axis])
points = np.stack([
sample(np.random.choice(sample_ax), np.random.choice([-1, 1]),
half_size, shrink_region) for _ in range(n)
])
return points
def get_side_face_centers(half_size, object_ori):
R_base_to_cube = Transform(np.zeros(3), object_ori).inverse()
z_cube = R_base_to_cube(np.array([0, 0, 1]))
axis = np.argmax(np.abs(z_cube))
points = []
for ax in range(3):
if ax != axis:
points.append(sample(ax, 1, half_size, np.zeros(3)))
points.append(sample(ax, -1, half_size, np.zeros(3)))
return np.array(points)
def add_tip_adjustments(self, obs, num_steps=50):
print("Appending to path....")
# tip_pos = self.path.tip_path[-1]
dir = 0.5 * (obs['goal_object_position'] - obs['object_position'])
cube_pos = self.cube_sequence[-1][:3]
cube_ori = p.getQuaternionFromEuler(self.cube_sequence[-1][3:])
grasp = self.path.grasp
if self.adjust_tip_ori:
yaxis = np.array([0, 1, 0])
goal_obj_yaxis = Rotation.from_quat(
obs['goal_object_orientation']).apply(yaxis)
obj_yaxis = Rotation.from_quat(cube_ori).apply(yaxis)
diff_quat = get_rotation_between_vecs(obj_yaxis, goal_obj_yaxis)
resolution = np.arange(0, 1, 1.0 / num_steps)
interp_quat = slerp(np.array([0, 0, 0, 1]), diff_quat, resolution)
warning_counter = 0
warning_tips = []
for i in range(num_steps):
translation = cube_pos + i / num_steps * dir
if self.adjust_tip_ori:
rotation = (Rotation.from_quat(interp_quat[i]) *
Rotation.from_quat(cube_ori)).as_quat()
else:
rotation = cube_ori
goal_tip_pos = Transform(translation, rotation)(grasp.cube_tip_pos)
q = obs['robot_position']
for j, tip in enumerate(goal_tip_pos):
q = self.env.pinocchio_utils.inverse_kinematics(j, tip, q)
if q is None:
q = self.joint_sequence[-1]
# print(f'[tip adjustments] warning: IK solution is not found for tip {j}. Using the last joint conf')
warning_counter += 1
if j not in warning_tips:
warning_tips.append(j)
break
if q is None:
print(
'[tip adjustments] warning: IK solution is not found for all tip positions.'
)
print(
f'[tip adjustments] aborting tip adjustments (loop {i} / {num_steps})'
)
break
target_cube_pose = np.concatenate(
[translation, p.getEulerFromQuaternion(rotation)])
self.cube_sequence.append(target_cube_pose)
self.joint_sequence.append(q)
self.path.tip_path.append(goal_tip_pos)
if warning_counter > 0:
print(
f'[tip adjustments] warning: IK solution is not found for {warning_counter} / {num_steps} times on tips {warning_tips}.'
)
def _get_cone_corners(self):
# approximate cone with an inscribed square
contact_normal = np.array([0, 0, 1])
fac = self.mu * np.sqrt(2) / 2
corners = []
transforms = []
for i in [-1, +1]:
for j in [-1, +1]:
corner = np.array([i * fac, j * fac, 1])
corner /= np.linalg.norm(corner)
q = get_rotation_between_vecs(contact_normal, corner)
corners.append(corner)
transforms.append(Transform(pos=np.zeros(3), ori=q))
return corners, transforms
def get_tiny_faces_heuristic_grasps(half_size, object_ori):
R_base_to_cube = Transform(np.zeros(3), object_ori).inverse()
z_cube = R_base_to_cube(np.array([0, 0, 1]))
axis = np.argmax(np.abs(z_cube))
above_point = []
for i in range(3):
if i == axis:
above_point.append(half_size[i] + 0.02)
else:
above_point.append(0)
point = np.stack([[0, half_size[1], 0], [0, -half_size[1], 0],
above_point])
return [point]
def object_grasped(self, grasp):
T_cube_to_base = Transform(self.obs['object_position'],
self.obs['object_orientation'])
target_tip_pos = T_cube_to_base(grasp.cube_tip_pos)
center_of_tips = np.mean(target_tip_pos, axis=0)
dist = np.linalg.norm(target_tip_pos - self.obs['robot_tip_positions'])
center_dist = np.linalg.norm(center_of_tips - np.mean(self.obs['robot_tip_positions'], axis=0))
object_is_grasped = center_dist < 0.07 and dist < 0.10
if object_is_grasped:
self.grasp_check_failed_count = 0
else:
self.grasp_check_failed_count += 1
print('incremented grasp_check_failed_count')
print(f'center_dist: {center_dist:.4f}\tdist: {dist:.4f}')
return self.grasp_check_failed_count < 5
class Grasp(object):
def __init__(self, cube_tip_pos, base_tip_pos, q, cube_pos, cube_quat,
T_cube_to_base, T_base_to_cube, valid_tips):
self.cube_tip_pos = cube_tip_pos
self.base_tip_pos = base_tip_pos
self.q = q
self.pos = cube_pos
self.quat = cube_quat
self.T_cube_to_base = T_cube_to_base
self.T_base_to_cube = T_base_to_cube
self.valid_tips = valid_tips
def update(self, cube_pos, cube_quat):
self.pos = cube_pos
self.quat = cube_quat
self.T_cube_to_base = Transform(self.pos, self.quat)
self.T_base_to_cube = self.T_cube_to_base.inverse()
self.base_tip_pos = self.T_cube_to_base(self.cube_tip_pos)
def get_long_side_face_centers(half_size, object_ori):
# check if z-axis (cube-frame) is in parralel with z-axis in base-frame
R_cube_to_base = Transform(np.zeros(3), object_ori)
base_z = R_cube_to_base(np.array([0, 0, 1]))
if np.argmax(np.abs(base_z)) == 2:
# z-axis (cube-frame) is in parallel with z-axis in base-frame
# --> sample from x-axis face
axis = 0
else:
# z-axis (cube-frame) is in parallel with the floor
# --> sample from z-axis face
axis = 2
points = [sample(axis, sign, half_size, np.zeros(3)) for sign in [-1, 1]]
vertical_points = [
sample(1, sign, half_size, np.zeros(3)) for sign in [-1, 1]
] # TEMP
return np.array(points), np.array(vertical_points)
def get_partial_grasp(self, execute=True, avg_pose=True):
"""
sample a 'partial' grasp for object centering
NOTE: Only use it for centering the object.
"""
if avg_pose:
obj_pos, obj_ori, self.obs, self.done = estimate_object_pose(
self.env, self.obs, steps=AVG_POSE_STEPS
)
else:
obj_pos = self.obs['object_position']
obj_ori = self.obs['object_orientation']
done = False
while not done:
grasp = grasping.sample_partial_grasp(self.env, obj_pos, obj_ori)
if len(grasp.valid_tips) == 1:
# NOTE: if only one of the tips is valid, check if
# a. the tip is far from origin than object
# b. angle between tip-to-origin vector and object's y-axis > 30 degree
dist = np.linalg.norm(grasp.base_tip_pos[:, :2], axis=1)
tip_id = np.argmax(dist)
origin_to_tip = grasp.base_tip_pos[tip_id, :2]
origin_to_tip /= np.linalg.norm(origin_to_tip)
cube_yaxis = Transform(np.array([0, 0, 0]), obj_ori)(np.array([0, 1, 0]))[:2]
cube_yaxis /= np.linalg.norm(cube_yaxis)
# cube_yaxis = Rotation.from_quat(obj_ori).apply(np.array([0, 1, 0])) # Is this same?
if dist[tip_id] > np.linalg.norm(obj_pos) and np.dot(origin_to_tip, cube_yaxis) < 1 / 2:
print('grasp.valid_tips', grasp.valid_tips)
done = True
else:
done = True
if grasp is not None and execute:
self.obs, self.done = grasping.execute_grasp_approach(
self.env, self.obs, grasp
)
if grasp is not None:
self.env.unwrapped.register_custom_log('target_object_pose', {'position': obj_pos, 'orientation': obj_ori})
self.env.unwrapped.register_custom_log('target_tip_pos', grasp.T_cube_to_base(grasp.cube_tip_pos))
self.env.unwrapped.save_custom_logs()
return grasp
def contact_from_tip_position(self, pos):
"""
Compute contact frame from tip positions in the cube
center of mass frame.
"""
outside = np.abs(pos) >= self.halfsize - 1e-5
sign = np.sign(pos)
contact = pos.copy()
normal = np.zeros(3)
if np.sum(outside) == 0:
outside[:] = True
dist = np.minimum(np.abs(pos - self.halfsize),
np.abs(pos + self.halfsize))
axes = np.argsort(dist)
ax = [ax for ax in axes if outside[ax]][0]
contact[ax] = sign[ax] * self.halfsize[ax]
normal[ax] = -sign[ax]
q = get_rotation_between_vecs(np.array([0, 0, 1]), normal)
return Transform(pos=contact, ori=q)
def sample_long_side_face(n,
half_size,
object_ori,
shrink_region=[0.0, 0.6, 0.0]):
'''sample from long side faces.
long side face == x-axis or z-axis in the cube frame
'''
# check if z-axis (cube-frame) is in parralel with z-axis in base-frame
R_cube_to_base = Transform(np.zeros(3), object_ori)
base_z = R_cube_to_base(np.array([0, 0, 1]))
if np.argmax(np.abs(base_z)) == 2:
# z-axis (cube-frame) is in parallel with z-axis in base-frame
# --> sample from x-axis face
axis = 0
else:
# z-axis (cube-frame) is in parallel with the floor
# --> sample from z-axis face
axis = 2
points = np.stack([
sample(axis, np.random.choice([-1, 1]), half_size, shrink_region)
for _ in range(n)
])
return points
def update_tip_force_markers(self, obs, tip_wrenches, force):
R_cube_to_base = Transform(np.zeros(3), obs['object_orientation'])
cube_force = R_cube_to_base(force)
self._set_markers([w[:3] for w in tip_wrenches],
obs['robot_tip_positions'], cube_force,
obs['object_position'])
def update(self, cube_pos, cube_quat):
self.pos = cube_pos
self.quat = cube_quat
self.T_cube_to_base = Transform(self.pos, self.quat)
self.T_base_to_cube = self.T_cube_to_base.inverse()
self.base_tip_pos = self.T_cube_to_base(self.cube_tip_pos)
class GraspSampler(object):
def __init__(self,
env,
pos,
quat,
slacky_collision=True,
halfsize=VIRTUAL_CUBOID_HALF_SIZE,
ignore_collision=False,
avoid_edge_faces=True,
yawing_grasp=False,
allow_partial_sol=False):
self.object_pos = pos
self.object_ori = quat
self.ik = env.pinocchio_utils.inverse_kinematics
self.id = env.platform.simfinger.finger_id
self.tip_ids = env.platform.simfinger.pybullet_tip_link_indices
self.link_ids = env.platform.simfinger.pybullet_link_indices
self.T_cube_to_base = Transform(pos, quat)
self.T_base_to_cube = self.T_cube_to_base.inverse()
self.env = env
self.ik_utils = IKUtils(env, yawing_grasp=yawing_grasp)
self.slacky_collision = slacky_collision
self._org_tips_init = np.array(
self.env.platform.forward_kinematics(INIT_JOINT_CONF))
self.halfsize = halfsize
self.tip_solver = CuboidForceClosureTest(halfsize, CoulombFriction(MU))
self.ignore_collision = ignore_collision
self.avoid_edge_faces = avoid_edge_faces
self.yawing_grasp = yawing_grasp
self.allow_partial_sol = allow_partial_sol
def _reject(self, points_base):
if not self.tip_solver.force_closure_test(self.T_cube_to_base,
points_base):
return True, None
if self.ignore_collision:
q = self.ik_utils._sample_ik(points_base)
elif self.allow_partial_sol:
q = self.ik_utils._sample_ik(points_base, allow_partial_sol=True)
else:
q = self.ik_utils._sample_no_collision_ik(
points_base,
slacky_collision=self.slacky_collision,
diagnosis=False)
if q is None:
return True, None
return False, q
def assign_positions_to_fingers(self, tips):
cost_to_inds = {}
for v in itertools.permutations([0, 1, 2]):
sorted_tips = tips[v, :]
cost = np.linalg.norm(sorted_tips - self._org_tips_init)
cost_to_inds[cost] = v
inds_sorted_by_cost = [
val
for key, val in sorted(cost_to_inds.items(), key=lambda x: x[0])
]
opt_inds = inds_sorted_by_cost[0]
opt_tips = tips[opt_inds, :]
# verbose output
return opt_tips, opt_inds, inds_sorted_by_cost
def get_feasible_grasps_from_tips(self, tips):
_, _, permutations_by_cost = self.assign_positions_to_fingers(tips)
for perm in permutations_by_cost:
ordered_tips = tips[perm, :]
should_reject, q = self._reject(ordered_tips)
if not should_reject:
# use INIT_JOINT_CONF for tip positions that were not solvable
valid_tips = [0, 1, 2]
if self.allow_partial_sol:
for i in range(3):
if q[i * 3] is None:
valid_tips.remove(i)
q[i * 3:(i + 1) *
3] = INIT_JOINT_CONF[i * 3:(i + 1) * 3]
yield Grasp(self.T_base_to_cube(ordered_tips), ordered_tips, q,
self.object_pos, self.object_ori,
self.T_cube_to_base, self.T_base_to_cube,
valid_tips)
def __call__(self, shrink_region=[0.0, 0.6, 0.0], max_retries=40):
retry = 0
print("sampling a random grasp...")
with keep_state(self.env):
while retry < max_retries:
print('[GraspSampler] retry count:', retry)
points = sample_long_side_face(3,
self.halfsize,
self.object_ori,
shrink_region=shrink_region)
tips = self.T_cube_to_base(points)
for grasp in self.get_feasible_grasps_from_tips(tips):
return grasp
retry += 1
raise RuntimeError('No feasible grasp is found.')
def get_heuristic_grasps(self):
grasps = get_all_heuristic_grasps(
self.halfsize,
self.object_ori,
avoid_edge_faces=self.avoid_edge_faces,
yawing_grasp=self.yawing_grasp,
is_level_1=self.env.info['difficulty'] == 1)
ret = []
with keep_state(self.env):
for points in grasps:
tips = self.T_cube_to_base(points)
# NOTE: we sacrifice a bit of speed by not using "yield", however,
# context manager doesn't work as we want if we use "yield".
# performance drop shouldn't be significant (get_feasible_grasps_from_tips only iterates 6 grasps!).
# for grasp in self.get_feasible_grasps_from_tips(tips):
# yield grasp
ret += [
grasp for grasp in self.get_feasible_grasps_from_tips(tips)
]
return ret