import grasping.annotation.utils as gau
if __name__ == '__main__':
import numpy as np
import robot_sim.end_effectors.grippers.robotiq85.robotiq85 as rtq85
import modeling.collision_model as cm
import visualization.panda.world as wd
base = wd.World(cam_pos=[.5, .5, .3], lookat_pos=[0, 0, 0])
gripper_s = rtq85.Robotiq85(enable_cc=True)
objcm = cm.CollisionModel("./objects/bunnysim.stl")
objcm.set_pos(np.array([.5, -.3, 1.2]))
objcm.attach_to(base)
objcm.show_localframe()
grasp_info_list = gau.define_grasp_with_rotation(
gripper_s,
objcm,
gl_jaw_center_pos=np.array([0, 0, 0]),
gl_jaw_center_z=np.array([1, 0, 0]),
gl_hndx=np.array([0, 1, 0]),
jaw_width=.04,
gl_rotation_ax=np.array([0, 0, 1]))
for grasp_info in grasp_info_list:
jaw_width, gl_jaw_center, pos, rotmat = grasp_info
gic = gripper_s.copy()
gic.fix_to(pos, rotmat)
gic.jaw_to(jaw_width)
print(pos, rotmat)
gic.gen_meshmodel().attach_to(base)
base.run()
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), name='ur3dual', enable_cc=True):
super().__init__(pos=pos, rotmat=rotmat, name=name)
this_dir, this_filename = os.path.split(__file__)
# left side
self.lft_body = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=np.zeros(1), name='lft_body_jl')
self.lft_body.jnts[1]['loc_pos'] = np.array([0.045, 0, 0.7296])
self.lft_body.jnts[2]['loc_pos'] = np.array([0.15, 0.101, 0.1704])
self.lft_body.jnts[2]['loc_rotmat'] = rm.rotmat_from_euler(-math.pi / 2.0, -math.pi / 2.0, 0)
self.lft_body.lnks[0]['name'] = "sda5f_lft_body"
self.lft_body.lnks[0]['loc_pos'] = np.array([0, 0, 0])
self.lft_body.lnks[0]['collisionmodel'] = cm.CollisionModel(
os.path.join(this_dir, "meshes", "base_link.stl"),
cdprimit_type="user_defined", expand_radius=.005,
userdefined_cdprimitive_fn=self._base_combined_cdnp)
self.lft_body.lnks[0]['rgba'] = [.7, .7, .7, 1.0]
self.lft_body.lnks[1]['name'] = "sda5f_lft_torso"
self.lft_body.lnks[1]['loc_pos'] = np.array([0, 0, 0])
self.lft_body.lnks[1]['collisionmodel'] = cm.CollisionModel(
os.path.join(this_dir, "meshes", "torso_link.stl"),
cdprimit_type="user_defined", expand_radius=.005,
userdefined_cdprimitive_fn=self._torso_combined_cdnp)
self.lft_body.lnks[1]['rgba'] = [.7, .7, .7, 1.0]
self.lft_body.reinitialize()
lft_arm_homeconf = np.zeros(7)
# lft_arm_homeconf[0] = math.pi / 3.0
# lft_arm_homeconf[1] = -math.pi * 1.0 / 3.0
# lft_arm_homeconf[2] = -math.pi * 2.0 / 3.0
# lft_arm_homeconf[3] = math.pi
# lft_arm_homeconf[4] = -math.pi / 2.0
self.lft_arm = sia.SIA5(pos=self.lft_body.jnts[-1]['gl_posq'],
rotmat=self.lft_body.jnts[-1]['gl_rotmatq'],
homeconf=lft_arm_homeconf,
enable_cc=False)
# lft hand offset (if needed)
self.lft_hnd_offset = np.zeros(3)
lft_hnd_pos, lft_hnd_rotmat = self.lft_arm.cvt_loc_tcp_to_gl(loc_pos=self.lft_hnd_offset)
self.lft_hnd = rtq.Robotiq85(pos=lft_hnd_pos,
rotmat=self.lft_arm.jnts[-1]['gl_rotmatq'],
enable_cc=False)
# right side
self.rgt_body = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=np.zeros(1), name='rgt_body_jl')
self.rgt_body.jnts[1]['loc_pos'] = np.array([0.045, 0, 0.7296]) # right from robot_s view
self.rgt_body.jnts[2]['loc_pos'] = np.array([0.15, -0.101, 0.1704])
self.rgt_body.jnts[2]['loc_rotmat'] = rm.rotmat_from_euler(math.pi / 2.0, -math.pi / 2.0, 0)
self.rgt_body.lnks[0]['name'] = "sda5f_rgt_body"
self.rgt_body.lnks[0]['loc_pos'] = np.array([0, 0, 0])
self.rgt_body.lnks[0]['meshfile'] = None
self.rgt_body.lnks[1]['name'] = "sda5f_rgt_torso"
self.rgt_body.lnks[1]['loc_pos'] = np.array([0, 0, 0])
self.rgt_body.lnks[1]['meshfile'] = None
self.rgt_body.reinitialize()
rgt_arm_homeconf = np.zeros(7)
# rgt_arm_homeconf[0] = -math.pi * 1.0 / 3.0
# rgt_arm_homeconf[1] = -math.pi * 2.0 / 3.0
# rgt_arm_homeconf[2] = math.pi * 2.0 / 3.0
# rgt_arm_homeconf[4] = math.pi / 2.0
self.rgt_arm = sia.SIA5(pos=self.rgt_body.jnts[-1]['gl_posq'],
rotmat=self.rgt_body.jnts[-1]['gl_rotmatq'],
homeconf=rgt_arm_homeconf,
enable_cc=False)
# rgt hand offset (if needed)
self.rgt_hnd_offset = np.zeros(3)
rgt_hnd_pos, rgt_hnd_rotmat = self.rgt_arm.cvt_loc_tcp_to_gl(loc_pos=self.rgt_hnd_offset)
# TODO replace using copy
self.rgt_hnd = rtq.Robotiq85(pos=rgt_hnd_pos,
rotmat=self.rgt_arm.jnts[-1]['gl_rotmatq'],
enable_cc=False)
# tool center point
# lft
self.lft_arm.tcp_jntid = -1
self.lft_arm.tcp_loc_pos = np.array([0, 0, .145])
self.lft_arm.tcp_loc_rotmat = np.eye(3)
# rgt
self.rgt_arm.tcp_jntid = -1
self.rgt_arm.tcp_loc_pos = np.array([0, 0, .145])
self.rgt_arm.tcp_loc_rotmat = np.eye(3)
# a list of detailed information about objects in hand, see CollisionChecker.add_objinhnd
self.lft_oih_infos = []
self.rgt_oih_infos = []
# collision detection
if enable_cc:
self.enable_cc()
self.manipulator_dict['rgt_arm'] = self.rgt_arm
self.manipulator_dict['lft_arm'] = self.lft_arm
self.manipulator_dict['rgt_hnd'] = self.rgt_arm # specify which hand is a gripper installed to
self.manipulator_dict['lft_hnd'] = self.lft_arm # specify which hand is a gripper installed to
self.hnd_dict['rgt_hnd'] = self.rgt_hnd
self.hnd_dict['lft_hnd'] = self.lft_hnd
rgba = rm.get_rgba_from_cmap(i)
gm.gen_sphere(contact_p0, radius=.002, rgba=rgba).attach_to(base)
gm.gen_arrow(contact_p0,
contact_p0 + contact_n0 * .01,
thickness=.0012,
rgba=rgba).attach_to(base)
# gm.gen_arrow(contact_p0, contact_p0-contact_n0*.1, thickness=.0012, rgba = rgba).attach_to(base)
gm.gen_sphere(contact_p1, radius=.002, rgba=rgba).attach_to(base)
# gm.gen_dashstick(contact_p0, contact_p1, thickness=.0012, rgba=rgba).attach_to(base)
gm.gen_arrow(contact_p1,
contact_p1 + contact_n1 * .01,
thickness=.0012,
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,
def __init__(self):
self.rtq85 = rtq85.Robotiq85()
self.rbt = rbts.UR3Dual()
def __init__(self,
pos=np.zeros(3),
rotmat=np.eye(3),
name='ur3dual',
enable_cc=True):
super().__init__(pos=pos, rotmat=rotmat, name=name)
this_dir, this_filename = os.path.split(__file__)
# left side
self.lft_body = jl.JLChain(pos=pos,
rotmat=rotmat,
homeconf=np.zeros(12),
name='lft_body_jl')
self.lft_body.jnts[0]['loc_pos'] = np.array([0.0, 0.0, 0.0])
self.lft_body.jnts[1]['loc_pos'] = np.array([-0.0, 0.0, 0.0])
self.lft_body.jnts[2]['loc_pos'] = np.array([0.0, 0.0, 0.0])
self.lft_body.jnts[3]['loc_pos'] = np.array([0.0, 0.0, 0.0])
self.lft_body.jnts[4]['loc_pos'] = np.array([-0.0, 0.0, 0.0])
self.lft_body.jnts[5]['loc_pos'] = np.array([0.0, 0.0, 0.0])
self.lft_body.jnts[6]['loc_pos'] = np.array([0.0, 0.0, 0.0])
self.lft_body.jnts[7]['loc_pos'] = np.array([-0.0, 0.0, 0.0])
self.lft_body.jnts[8]['loc_pos'] = np.array([0.0, 0.0, 0.0])
self.lft_body.jnts[9]['loc_pos'] = np.array([0.0, 0.0, 0.0])
self.lft_body.jnts[10]['loc_pos'] = np.array([-0.0, 0.0, 0.0])
self.lft_body.jnts[11]['loc_pos'] = np.array([0.0, 0.0, 0.0])
self.lft_body.jnts[12]['loc_pos'] = np.array([0.0, 0.0, 0.0])
self.lft_body.jnts[13]['loc_pos'] = np.array(
[.0, .258485281374, 1.61051471863])
self.lft_body.jnts[13]['loc_rotmat'] = rm.rotmat_from_euler(
-3.0 * math.pi / 4.0, 0, math.pi, 'rxyz')
# body
self.lft_body.lnks[0]['name'] = "ur3_dual_lft_body"
self.lft_body.lnks[0]['loc_pos'] = np.array([0, 0, 0])
self.lft_body.lnks[0]['collisionmodel'] = cm.CollisionModel(
os.path.join(this_dir, "meshes", "ur3_dual_base.stl"),
cdprimit_type="user_defined",
expand_radius=.005,
userdefined_cdprimitive_fn=self._base_combined_cdnp)
self.lft_body.lnks[0]['rgba'] = [.3, .3, .3, 1.0]
# columns
self.lft_body.lnks[1]['name'] = "ur3_dual_back_rgt_column"
self.lft_body.lnks[1]['loc_pos'] = np.array([-0.41, -0.945, 0])
self.lft_body.lnks[1]['collisionmodel'] = cm.CollisionModel(
os.path.join(this_dir, "meshes", "ur3_dual_column2400x60x60.stl"))
self.lft_body.lnks[2]['name'] = "ur3_dual_back_lft_column"
self.lft_body.lnks[2]['loc_pos'] = np.array([-0.41, 0.945, 0])
self.lft_body.lnks[2]['collisionmodel'] = cm.CollisionModel(
os.path.join(this_dir, "meshes", "ur3_dual_column2400x60x60.stl"))
self.lft_body.lnks[3]['name'] = "ur3_dual_front_rgt_column"
self.lft_body.lnks[3]['loc_pos'] = np.array([0.73, -0.945, 0])
self.lft_body.lnks[3]['collisionmodel'] = cm.CollisionModel(
os.path.join(this_dir, "meshes", "ur3_dual_column2400x60x60.stl"))
self.lft_body.lnks[4]['name'] = "ur3_dual_front_lft_column"
self.lft_body.lnks[4]['loc_pos'] = np.array([0.73, 0.945, 0])
self.lft_body.lnks[4]['collisionmodel'] = cm.CollisionModel(
os.path.join(this_dir, "meshes", "ur3_dual_column2400x60x60.stl"))
# x_rows
self.lft_body.lnks[5]['name'] = "ur3_dual_up_rgt_xrow"
self.lft_body.lnks[5]['loc_pos'] = np.array([-0.38, -0.945, 2.37])
self.lft_body.lnks[5]['loc_rotmat'] = rm.rotmat_from_euler(
0, math.pi / 2, 0)
self.lft_body.lnks[5]['collisionmodel'] = cm.CollisionModel(
os.path.join(this_dir, "meshes", "ur3_dual_column1080x60x60.stl"))
self.lft_body.lnks[6]['name'] = "ur3_dual_bottom_rgt_xrow"
self.lft_body.lnks[6]['loc_pos'] = np.array([-0.38, -0.945, 1.07])
self.lft_body.lnks[6]['loc_rotmat'] = rm.rotmat_from_euler(
0, math.pi / 2, 0)
self.lft_body.lnks[6]['collisionmodel'] = cm.CollisionModel(
os.path.join(this_dir, "meshes", "ur3_dual_column1080x60x60.stl"))
self.lft_body.lnks[7]['name'] = "ur3_dual_up_lft_xrow"
self.lft_body.lnks[7]['loc_pos'] = np.array([-0.38, 0.945, 2.37])
self.lft_body.lnks[7]['loc_rotmat'] = rm.rotmat_from_euler(
0, math.pi / 2, 0)
self.lft_body.lnks[7]['collisionmodel'] = cm.CollisionModel(
os.path.join(this_dir, "meshes", "ur3_dual_column1080x60x60.stl"))
self.lft_body.lnks[8]['name'] = "ur3_dual_bottom_lft_xrow"
self.lft_body.lnks[8]['loc_pos'] = np.array([-0.38, 0.945, 1.07])
self.lft_body.lnks[8]['loc_rotmat'] = rm.rotmat_from_euler(
0, math.pi / 2, 0)
self.lft_body.lnks[8]['collisionmodel'] = cm.CollisionModel(
os.path.join(this_dir, "meshes", "ur3_dual_column1080x60x60.stl"))
# y_rows
self.lft_body.lnks[9]['name'] = "ur3_dual_back_up_yrow"
self.lft_body.lnks[9]['loc_pos'] = np.array([-0.41, -0.915, 2.37])
self.lft_body.lnks[9]['loc_rotmat'] = rm.rotmat_from_euler(
-math.pi / 2, 0, 0)
self.lft_body.lnks[9]['collisionmodel'] = cm.CollisionModel(
os.path.join(this_dir, "meshes", "ur3_dual_column1830x60x60.stl"))
self.lft_body.lnks[10]['name'] = "ur3_dual_back_bottom_yrow"
self.lft_body.lnks[10]['loc_pos'] = np.array([-0.41, -0.915, 0.35])
self.lft_body.lnks[10]['loc_rotmat'] = rm.rotmat_from_euler(
-math.pi / 2, 0, 0)
self.lft_body.lnks[10]['collisionmodel'] = cm.CollisionModel(
os.path.join(this_dir, "meshes", "ur3_dual_column1830x60x60.stl"))
self.lft_body.lnks[11]['name'] = "ur3_dual_front_up_yrow"
self.lft_body.lnks[11]['loc_pos'] = np.array([0.73, -0.915, 2.37])
self.lft_body.lnks[11]['loc_rotmat'] = rm.rotmat_from_euler(
-math.pi / 2, 0, 0)
self.lft_body.lnks[11]['collisionmodel'] = cm.CollisionModel(
os.path.join(this_dir, "meshes", "ur3_dual_column1830x60x60.stl"))
# table TODO update using vision sensors
self.lft_body.lnks[12]['name'] = "ur3_dual_table"
self.lft_body.lnks[12]['loc_pos'] = np.array([0.36, 0.0, 1.046])
self.lft_body.lnks[12]['loc_rotmat'] = rm.rotmat_from_euler(
0, 0, math.pi / 2)
self.lft_body.lnks[12]['collisionmodel'] = cm.CollisionModel(
os.path.join(this_dir, "meshes", "ur3_dual_table1820x54x800.stl"))
self.lft_body.lnks[12]['rgba'] = [.9, .77, .52, 1.0]
self.lft_body.reinitialize()
lft_arm_homeconf = np.zeros(6)
lft_arm_homeconf[0] = math.pi / 3.0
lft_arm_homeconf[1] = -math.pi * 1.0 / 3.0
lft_arm_homeconf[2] = -math.pi * 2.0 / 3.0
lft_arm_homeconf[3] = math.pi
lft_arm_homeconf[4] = -math.pi / 2.0
self.lft_arm = ur.UR3(pos=self.lft_body.jnts[-1]['gl_posq'],
rotmat=self.lft_body.jnts[-1]['gl_rotmatq'],
homeconf=lft_arm_homeconf,
enable_cc=False)
# lft hand ftsensor
self.lft_ft_sensor = jl.JLChain(
pos=self.lft_arm.jnts[-1]['gl_posq'],
rotmat=self.lft_arm.jnts[-1]['gl_rotmatq'],
homeconf=np.zeros(0),
name='lft_ft_sensor_jl')
self.lft_ft_sensor.jnts[1]['loc_pos'] = np.array([.0, .0, .0484])
self.lft_ft_sensor.lnks[0]['name'] = "ur3_dual_lft_ft_sensor"
self.lft_ft_sensor.lnks[0]['loc_pos'] = np.array([0, 0, 0])
self.lft_ft_sensor.lnks[0]['collisionmodel'] = cm.gen_stick(
spos=self.lft_ft_sensor.jnts[0]['loc_pos'],
epos=self.lft_ft_sensor.jnts[1]['loc_pos'],
thickness=.067,
rgba=[.2, .3, .3, 1],
sections=24)
self.lft_ft_sensor.reinitialize()
# lft hand
self.lft_hnd = rtq.Robotiq85(
pos=self.lft_ft_sensor.jnts[-1]['gl_posq'],
rotmat=self.lft_ft_sensor.jnts[-1]['gl_rotmatq'],
enable_cc=False)
# rigth side
self.rgt_body = jl.JLChain(pos=pos,
rotmat=rotmat,
homeconf=np.zeros(0),
name='rgt_body_jl')
self.rgt_body.jnts[1]['loc_pos'] = np.array(
[.0, -.258485281374, 1.61051471863]) # right from robot_s view
self.rgt_body.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler(
3.0 * math.pi / 4.0, .0, .0) # left from robot_s view
self.rgt_body.lnks[0]['name'] = "ur3_dual_rgt_body"
self.rgt_body.lnks[0]['loc_pos'] = np.array([0, 0, 0])
self.rgt_body.lnks[0]['meshfile'] = None
self.rgt_body.lnks[0]['rgba'] = [.3, .3, .3, 1.0]
self.rgt_body.reinitialize()
rgt_arm_homeconf = np.zeros(6)
rgt_arm_homeconf[0] = -math.pi * 1.0 / 3.0
rgt_arm_homeconf[1] = -math.pi * 2.0 / 3.0
rgt_arm_homeconf[2] = math.pi * 2.0 / 3.0
rgt_arm_homeconf[4] = math.pi / 2.0
self.rgt_arm = ur.UR3(pos=self.rgt_body.jnts[-1]['gl_posq'],
rotmat=self.rgt_body.jnts[-1]['gl_rotmatq'],
homeconf=rgt_arm_homeconf,
enable_cc=False)
# rgt hand ft sensor
self.rgt_ft_sensor = jl.JLChain(
pos=self.rgt_arm.jnts[-1]['gl_posq'],
rotmat=self.rgt_arm.jnts[-1]['gl_rotmatq'],
homeconf=np.zeros(0),
name='rgt_ft_sensor_jl')
self.rgt_ft_sensor.jnts[1]['loc_pos'] = np.array([.0, .0, .0484])
self.rgt_ft_sensor.lnks[0]['name'] = "ur3_dual_rgt_ft_sensor"
self.rgt_ft_sensor.lnks[0]['loc_pos'] = np.array([0, 0, 0])
self.rgt_ft_sensor.lnks[0]['collisionmodel'] = cm.gen_stick(
spos=self.rgt_ft_sensor.jnts[0]['loc_pos'],
epos=self.rgt_ft_sensor.jnts[1]['loc_pos'],
thickness=.067,
rgba=[.2, .3, .3, 1],
sections=24)
self.rgt_ft_sensor.reinitialize()
# TODO replace using copy
self.rgt_hnd = rtq.Robotiq85(
pos=self.rgt_ft_sensor.jnts[-1]['gl_posq'],
rotmat=self.rgt_ft_sensor.jnts[-1]['gl_rotmatq'],
enable_cc=False)
# tool center point
# lft
self.lft_arm.tcp_jntid = -1
self.lft_arm.tcp_loc_pos = self.lft_ft_sensor.jnts[-1][
'loc_pos'] + self.lft_hnd.jaw_center_pos
self.lft_arm.tcp_loc_rotmat = self.lft_ft_sensor.jnts[-1][
'loc_rotmat'].dot(self.lft_hnd.jaw_center_rotmat)
# rgt
self.rgt_arm.tcp_jntid = -1
self.rgt_arm.tcp_loc_pos = self.lft_ft_sensor.jnts[-1][
'loc_pos'] + self.lft_hnd.jaw_center_pos
self.rgt_arm.tcp_loc_rotmat = self.lft_ft_sensor.jnts[-1][
'loc_rotmat'].dot(self.lft_hnd.jaw_center_rotmat)
# a list of detailed information about objects in hand, see CollisionChecker.add_objinhnd
self.lft_oih_infos = []
self.rgt_oih_infos = []
# collision detection
if enable_cc:
self.enable_cc()
# component map
self.manipulator_dict['rgt_arm'] = self.rgt_arm
self.manipulator_dict['lft_arm'] = self.lft_arm
self.manipulator_dict[
'rgt_hnd'] = self.rgt_arm # specify which hand is a gripper installed to
self.manipulator_dict[
'lft_hnd'] = self.lft_arm # specify which hand is a gripper installed to
self.manipulator_dict[
'rgt_ftsensor'] = self.rgt_arm # specify which hand is a gripper installed to
self.manipulator_dict[
'lft_ftsensor'] = self.lft_arm # specify which hand is a gripper installed to
self.hnd_dict['rgt_hnd'] = self.rgt_hnd
self.hnd_dict['lft_hnd'] = self.lft_hnd
self.hnd_dict['rgt_arm'] = self.rgt_hnd
self.hnd_dict['lft_arm'] = self.lft_hnd
self.hnd_dict['rgt_ftsensor'] = self.rgt_hnd
self.hnd_dict['lft_ftsensor'] = self.lft_hnd
self.ft_sensor_dict['rgt_ftsensor'] = self.rgt_ft_sensor
self.ft_sensor_dict['lft_ftsensor'] = self.lft_ft_sensor
self.ft_sensor_dict['rgt_arm'] = self.rgt_ft_sensor
self.ft_sensor_dict['lft_arm'] = self.lft_ft_sensor
self.ft_sensor_dict['rgt_hnd'] = self.rgt_ft_sensor
self.ft_sensor_dict['lft_hnd'] = self.lft_ft_sensor
import numpy as np
import visualization.panda.world as wd
import modeling.geometric_model as gm
import modeling.collision_model as cm
import grasping.annotation.utils as gu
import robot_sim.end_effectors.grippers.robotiq85.robotiq85 as rtq85
import basis.robot_math as rm
base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, 0])
gm.gen_frame().attach_to(base)
# object
object_box = cm.gen_box(extent=[.02, .06, .1])
object_box.set_rgba([.7, .5, .3, .7])
object_box.attach_to(base)
# hnd_s
rtq85_s = rtq85.Robotiq85()
# rtq85_s.gen_meshmodel().attach_to(base)
# base.run()
# gl_jaw_center_z = rm.rotmat_from_axangle(np.array([0,1,0]), -math.pi/6).dot(np.array([-1,0,0]))
# grasp_info_list = gu.define_grasp(rtq85_s, object_box,
# gl_jaw_center_pos=np.array([0,0,0]),
# gl_jaw_center_z=gl_jaw_center_z,
# gl_jaw_center_y=np.array([0,1,0]),
# jawwidth=.065,
# toggle_flip=True,
# toggle_debug=True)
grasp_info_list = gu.define_grasp_with_rotation(
rtq85_s,
object_box,
gl_jaw_center_pos=np.array([0, 0, 0]),
gl_jaw_center_z=np.array([-1, 0, 0]),