def gen_rotmat_list(nsample=None):
rotmats = rm.gen_icorotmats(icolevel=2,
rotation_interval=math.radians(30),
crop_normal=np.array([0, 0, -1]),
crop_angle=np.pi / 3,
toggle_flat=True)
return_rotmat = []
for rotmat in rotmats:
if rm.angle_between_vectors(rotmat[:, 0], np.array([0, 0, -1])) < np.pi / 3:
return_rotmat.append(rotmat)
nreturn = len(return_rotmat)
if nsample is not None and nsample < nreturn:
return return_rotmat[0:nreturn:int(nreturn / nsample)]
return return_rotmat
def define_pushing(hnd_s,
objcm,
gl_surface_pos,
gl_surface_normal,
cone_angle=math.radians(30),
icosphere_level=2,
local_rotation_interval=math.radians(45),
toggle_debug=False):
"""
:param hnd_s:
:param objcm:
:param gl_surface_pos: used as cone tip
:param gl_surface_normal: used as cone's main axis
:param cone_angle: pushing poses will be randomized in this cone
:param icosphere_levle: 2
:param local_rotation_interval: discretize the rotation around the local axis of each pushing pose
:return:
author: weiwei
date: 20220308
"""
push_info_list = []
collided_push_info_list = []
pushing_icorotmats = rm.gen_icorotmats(
icolevel=icosphere_level,
crop_angle=cone_angle,
crop_normal=gl_surface_normal,
rotation_interval=local_rotation_interval,
toggle_flat=True)
for pushing_rotmat in pushing_icorotmats:
push_info = hnd_s.push_at(gl_push_pos=gl_surface_pos,
gl_push_rotmat=pushing_rotmat)
if not hnd_s.is_mesh_collided([objcm]):
push_info_list.append(push_info)
else:
collided_push_info_list.append(push_info)
if toggle_debug:
for push_info in collided_push_info_list:
gl_tip_pos, gl_tip_rotmat, hnd_pos, hnd_rotmat = push_info
hnd_s.fix_to(hnd_pos, hnd_rotmat)
hnd_s.gen_meshmodel(rgba=[1, 0, 0, .3]).attach_to(base)
for push_info in push_info_list:
gl_tip_pos, gl_tip_rotmat, hnd_pos, hnd_rotmat = push_info
hnd_s.fix_to(hnd_pos, hnd_rotmat)
hnd_s.gen_meshmodel(rgba=[0, 1, 0, .3]).attach_to(base)
base.run()
return push_info_list
def search_reachable_configuration(rbt_s,
ee_s,
component_name,
tgt_pos,
cone_axis,
cone_angle=0,
rotation_interval=np.radians(22.5),
obstacle_list=[],
seed_jnt_values=None,
toggle_debug=False) -> np.typing.NDArray:
"""
search reachable configuration in a cone
when the cone_angle is 0, the function degenerates into a search around the cone_axis
:param rbt_s: instance of a robot
:param ee_s: instance of an end-effector
:param tgt_pos:
:param cone_axis:
:param cone_angle:
:param granularity:
:param obstacle_list
:return:
author: weiwei
date: 20220404
"""
jnt_values_bk = rbt_s.get_jnt_values(component_name=component_name)
if seed_jnt_values is None:
seed_jnt_values = jnt_values_bk
rotmat_list = []
if cone_angle != 0:
rotmat_list = rm.gen_icorotmats(icolevel=3,
rotation_interval=rotation_interval,
crop_normal=-cone_axis,
crop_angle=cone_angle,
toggle_flat=True)
else:
rotmat = rm.rotmat_from_axangle([0, 0, 1], 0).dot(rm.rotmat_from_normal(cone_axis))
for angle in np.linspace(0, np.pi * 2, int(np.pi * 2 / rotation_interval), endpoint=False):
rotmat_list.append(rm.rotmat_from_axangle([0, 0, 1], -angle).dot(rotmat))
print("======new search!")
for i, rotmat in enumerate(rotmat_list):
jnt_values = rbt_s.ik(component_name=component_name,
tgt_pos=tgt_pos,
tgt_rotmat=rotmat,
seed_jnt_values=seed_jnt_values)
if jnt_values is not None:
rbt_s.fk(jnt_values=jnt_values)
if rbt_s.is_collided(obstacle_list=obstacle_list):
if toggle_debug:
rbt_s.gen_meshmodel(rgba=[.9, .5, 0, .3]).attach_to(base)
else:
if toggle_debug:
rbt_s.gen_meshmodel().attach_to(base)
if not toggle_debug:
rbt_s.fk(component_name=component_name,
jnt_values=jnt_values_bk)
print("times tried ", i)
return jnt_values
else:
if toggle_debug:
ee_s.grip_at_with_jcpose(gl_jaw_center_pos=tgt_pos,
gl_jaw_center_rotmat=rotmat,
jaw_width=0)
ee_s.gen_meshmodel(rgba=[1, 0, 0, .3]).attach_to(base)
rbt_s.fk(component_name=component_name, jnt_values=jnt_values_bk)
return None