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)
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
