def plan_grasps(hnd_s,
objcm,
angle_between_contact_normals=math.radians(160),
openning_direction = 'loc_x',
rotation_interval=math.radians(22.5),
max_samples=100,
min_dist_between_sampled_contact_points=.005,
contact_offset=.002):
"""
:param objcm:
:param hnd_s:
:param angle_between_contact_normals:
:param openning_direction: 'loc_x' or 'loc_y' depending on gripper types
:param rotation_granularity:
:param max_samples:
:param min_dist_between_sampled_contact_points:
:param contact_offset: offset at the cotnact to avoid being closely in touch with object surfaces
:return: a list [[jawwidth, gl_jaw_center_pos, pos, rotmat], ...]
"""
contact_pairs = plan_contact_pairs(objcm,
max_samples=max_samples,
min_dist_between_sampled_contact_points=min_dist_between_sampled_contact_points,
angle_between_contact_normals=angle_between_contact_normals)
grasp_info_list = []
import modeling.geometric_model as gm
for i, cp in enumerate(contact_pairs):
print(f"{i} of {len(contact_pairs)} done!")
contact_p0, contact_n0 = cp[0]
contact_p1, contact_n1 = cp[1]
contact_center = (contact_p0 + contact_p1) / 2
jaw_width = np.linalg.norm(contact_p0 - contact_p1) + contact_offset * 2
if jaw_width > hnd_s.jawwidth_rng[1]:
continue
if openning_direction == 'loc_x':
jaw_center_x = contact_n0
jaw_center_z = rm.orthogonal_vector(contact_n0)
jaw_center_y = np.cross(jaw_center_z, jaw_center_x)
elif openning_direction == 'loc_y':
jaw_center_y = contact_n0
jaw_center_z = rm.orthogonal_vector(contact_n0)
else:
raise ValueError("Openning direction must be loc_x or loc_y!")
grasp_info_list += gu.define_grasp_with_rotation(hnd_s,
objcm,
gl_jaw_center_pos=contact_center,
gl_jaw_center_z=jaw_center_z,
gl_jaw_center_y=jaw_center_y,
jaw_width=jaw_width,
gl_rotation_ax=contact_n0,
rotation_interval=rotation_interval,
toggle_flip=True)
return grasp_info_list
def gen_dashtorus(axis=np.array([1, 0, 0]),
portion=.5,
center=np.array([0, 0, 0]),
radius=0.1,
thickness=0.005,
lsolid=None,
lspace=None,
sections=8,
discretization=24):
"""
:param axis: the circ arrow will rotate around this axis 1x3 nparray
:param portion: 0.0~1.0
:param center: the center position of the circ 1x3 nparray
:param radius:
:param thickness:
:param lsolid: length of solid
:param lspace: length of space
:param sections: # of discretized sectors used to approximate a cylindrical stick
:param discretization: number sticks used for approximating a torus
:return:
author: weiwei
date: 20200602
"""
assert (0 <= portion <= 1)
solidweight = 1.6
spaceweight = 1.07
if not lsolid:
lsolid = thickness * solidweight
if not lspace:
lspace = thickness * spaceweight
unit_axis = rm.unit_vector(axis)
starting_vector = rm.orthogonal_vector(unit_axis)
min_discretization_value = math.ceil(2 * math.pi / (lsolid + lspace))
if discretization < min_discretization_value:
discretization = min_discretization_value
nsections = math.floor(portion * 2 * math.pi * radius / (lsolid + lspace))
vertices = np.empty((0, 3))
faces = np.empty((0, 3))
for i in range(0, nsections): # TODO wrap up end
torus_sec = gen_torus(
axis=axis,
starting_vector=rm.rotmat_from_axangle(
axis,
2 * math.pi * portion / nsections * i).dot(starting_vector),
portion=portion / nsections * lsolid / (lsolid + lspace),
center=center,
radius=radius,
thickness=thickness,
sections=sections,
discretization=discretization)
torus_sec_faces = torus_sec.faces + len(vertices)
vertices = np.vstack((vertices, torus_sec.vertices))
faces = np.vstack((faces, torus_sec_faces))
return trm.Trimesh(vertices=vertices, faces=faces)
bowl_model.set_rgba([.3, .3, .3, .3])
bowl_model.set_rotmat(rm.rotmat_from_euler(math.pi, 0, 0))
# bowl_model.attach_to(base)
pn_direction = np.array([0, 0, -1])
bowl_samples, bowl_sample_normals = bowl_model.sample_surface(
toggle_option='normals', radius=.002)
selection = bowl_sample_normals.dot(-pn_direction) > .1
bowl_samples = bowl_samples[selection]
bowl_sample_normals = bowl_sample_normals[selection]
tree = cKDTree(bowl_samples)
surface = rbfs.RBFSurface(bowl_samples[:, :2], bowl_samples[:, 2])
surface.get_gometricmodel(rgba=[.3, .3, .3, .3]).attach_to(base)
pt_direction = rm.orthogonal_vector(pn_direction, toggle_unit=True)
tmp_direction = np.cross(pn_direction, pt_direction)
plane_rotmat = np.column_stack((pt_direction, tmp_direction, pn_direction))
homomat = np.eye(4)
homomat[:3, :3] = plane_rotmat
homomat[:3, 3] = np.array([-.07, -.03, .1])
twod_plane = gm.gen_box(np.array([.2, .2, .001]),
homomat=homomat,
rgba=[1, 1, 1, .3])
twod_plane.attach_to(base)
circle_radius = .05
line_segs = [[homomat[:3, 3], homomat[:3, 3] + pt_direction * .05],
[
homomat[:3, 3] + pt_direction * .05,
homomat[:3, 3] + pt_direction * .05 + tmp_direction * .05
base = wd.World(cam_pos=np.array([-.3,-.7,.42]), lookat_pos=np.array([0,0,0]))
# gm.gen_frame().attach_to(base)
bowl_model = cm.CollisionModel(initor="./objects/bowl.stl")
bowl_model.set_rgba([.3,.3,.3,1])
bowl_model.set_rotmat(rm.rotmat_from_euler(math.pi,0,0))
bowl_model.attach_to(base)
pn_direction = np.array([0, 0, -1])
bowl_samples, bowl_sample_normals = bowl_model.sample_surface(toggle_option='normals', radius=.002)
selection = bowl_sample_normals.dot(-pn_direction)>.1
bowl_samples = bowl_samples[selection]
bowl_sample_normals=bowl_sample_normals[selection]
tree = cKDTree(bowl_samples)
pt_direction = rm.orthogonal_vector(pn_direction, toggle_unit=True)
tmp_direction = np.cross(pn_direction, pt_direction)
plane_rotmat = np.column_stack((pt_direction, tmp_direction, pn_direction))
homomat=np.eye(4)
homomat[:3,:3] = plane_rotmat
homomat[:3,3] = np.array([-.07,-.03,.1])
twod_plane = gm.gen_box(np.array([.2, .2, .001]), homomat=homomat, rgba=[1,1,1,.3])
twod_plane.attach_to(base)
circle_radius=.05
line_segs = [[homomat[:3,3], homomat[:3,3]+pt_direction*.07], [homomat[:3,3]+pt_direction*.07, homomat[:3,3]+pt_direction*.07+tmp_direction*.07],
[homomat[:3,3]+pt_direction*.07+tmp_direction*.07, homomat[:3,3]+tmp_direction*.07], [homomat[:3,3]+tmp_direction*.07, homomat[:3,3]]]
# gm.gen_linesegs(line_segs).attach_to(base)
for sec in line_segs:
gm.gen_stick(spos=sec[0], epos=sec[1], rgba=[0, 0, 0, 1], thickness=.002, type='round').attach_to(base)
epos = (line_segs[0][1]-line_segs[0][0])*.7+line_segs[0][0]
def gen_circarrow(axis=np.array([1, 0, 0]),
starting_vector=None,
portion=0.3,
center=np.array([0, 0, 0]),
radius=0.005,
thickness=0.0015,
sections=8,
discretization=24):
"""
:param axis: the circ arrow will rotate around this axis 1x3 nparray
:param portion: 0.0~1.0
:param center: the center position of the circ 1x3 nparray
:param radius:
:param thickness:
:param rgba:
:param discretization: number sticks used for approximation
:return:
author: weiwei
date: 20200602
"""
unitaxis = rm.unit_vector(axis)
if starting_vector is None:
starting_vector = rm.orthogonal_vector(unitaxis)
else:
starting_vector = rm.unit_vector(starting_vector)
starting_pos = starting_vector * radius + center
discretizedangle = 2 * math.pi / discretization
ndist = int(portion * discretization)
# gen the last arrow first
# gen the remaining torus
if ndist > 0:
lastpos = center + np.dot(
rm.rotmat_from_axangle(unitaxis, (ndist - 1) * discretizedangle),
starting_vector) * radius
nxtpos = center + np.dot(
rm.rotmat_from_axangle(unitaxis, ndist * discretizedangle),
starting_vector) * radius
arrow = gen_arrow(spos=lastpos,
epos=nxtpos,
thickness=thickness,
sections=sections,
sticktype="round")
vertices = arrow.vertices
faces = arrow.faces
lastpos = starting_pos
for i in range(1 * np.sign(ndist), ndist, 1 * np.sign(ndist)):
nxtpos = center + np.dot(
rm.rotmat_from_axangle(unitaxis, i * discretizedangle),
starting_vector) * radius
stick = gen_stick(spos=lastpos,
epos=nxtpos,
thickness=thickness,
sections=sections,
type="round")
stickfaces = stick.faces + len(vertices)
vertices = np.vstack((vertices, stick.vertices))
faces = np.vstack((faces, stickfaces))
lastpos = nxtpos
return trm.Trimesh(vertices=vertices, faces=faces)
else:
return trm.Trimesh()
# bowl_model.attach_to(base)
# base.run()
tree = cKDTree(bowl_samples)
point_id = 7000
nearby_sample_ids = tree.query_ball_point(bowl_samples[point_id, :], 10)
nearby_samples = bowl_samples[nearby_sample_ids]
colors = np.tile(np.array([1, 0, 0, 1]), (len(nearby_samples), 1))
print(nearby_samples.shape)
print(colors.shape)
# nearby_samples_withcolor = np.column_stack((nearby_samples, colors))
# gm.GeometricModel(nearby_samples_withcolor).attach_to(base)
pcdu.show_pcd(nearby_samples, rgba=(1, 0, 0, 1))
plane_center, plane_normal = rm.fit_plane(nearby_samples)
plane_tangential = rm.orthogonal_vector(plane_normal)
plane_tmp = np.cross(plane_normal, plane_tangential)
plane_rotmat = np.column_stack((plane_tangential, plane_tmp, plane_normal))
nearby_samples_on_xy = plane_rotmat.T.dot((nearby_samples - plane_center).T).T
surface = gs.MixedGaussianSurface(nearby_samples_on_xy[:, :2],
nearby_samples_on_xy[:, 2],
n_mix=1)
# t_npt_on_xy = plane_rotmat.T.dot(t_npt - plane_center)
# projected_t_npt_z_on_xy = surface.get_zdata(np.array([t_npt_on_xy[:2]]))
# projected_t_npt_on_xy = np.array([t_npt_on_xy[0], t_npt_on_xy[1], projected_t_npt_z_on_xy[0]])
# projected_point = plane_rotmat.dot(projected_t_npt_on_xy) + plane_center
surface_gm = surface.get_gometricmodel([[-50, 50], [-50, 50]],
rgba=[.5, .7, 1, 1])
surface_gm.setpos(plane_center)
surface_gm.setrotmat(plane_rotmat)
surface_gm.reparentTo(base.render)
rgba=rgba).attach_to(base)
# gm.gen_dasharrow(contact_p1, contact_p1+contact_n1*.03, thickness=.0012, rgba=rgba).attach_to(base)
# base.run()
gripper_s = rtq85.Robotiq85()
contact_offset = .002
grasp_info_list = []
for i, cp in enumerate(contact_pairs):
print(f"{i} of {len(contact_pairs)} done!")
contact_p0, contact_n0 = cp[0]
contact_p1, contact_n1 = cp[1]
contact_center = (contact_p0 + contact_p1) / 2
jaw_width = np.linalg.norm(contact_p0 - contact_p1) + contact_offset * 2
if jaw_width > gripper_s.jawwidth_rng[1]:
continue
hndy = contact_n0
hndz = rm.orthogonal_vector(contact_n0)
grasp_info_list += gu.define_grasp_with_rotation(
gripper_s,
object_bunny,
gl_jaw_center_pos=contact_center,
gl_jaw_center_z=hndz,
gl_jaw_center_y=hndy,
jaw_width=jaw_width,
gl_rotation_ax=hndy,
rotation_interval=math.radians(30),
toggle_flip=True)
for grasp_info in grasp_info_list:
aw_width, gl_jaw_center, hnd_pos, hnd_rotmat = grasp_info
gripper_s.fix_to(hnd_pos, hnd_rotmat)
gripper_s.jaw_to(aw_width)
gripper_s.gen_meshmodel().attach_to(base)
