import visualization.panda.world as wd
import modeling.geometric_model as gm
import modeling.collision_model as cm
import grasping.planning.antipodal as gpa
import pickle
import robot_sim.end_effectors.gripper.robotiq85.robotiq85 as rtq85
base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, 0], auto_cam_rotate=True)
gm.gen_frame().attach_to(base)
object_bunny = cm.CollisionModel("objects/pblcm_cropped_8_2_20000_cvt.stl")
object_bunny.set_rgba([.9, .75, .35, .3])
object_bunny.attach_to(base)
# hnd_s
gripper_s = rtq85.Robotiq85()
# base.run()
# gripper_s.gen_meshmodel(toggle_jntscs=True, toggle_tcpcs=True).attach_to(base)
# base.run()
grasp_info_list = gpa.plan_grasps(gripper_s,
object_bunny,
openning_direction='loc_y',
max_samples=100,
min_dist_between_sampled_contact_points=.01)
for grasp_info in grasp_info_list:
aw_width, gl_jaw_center, gl_jaw_rotmat, 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)
# break
base.run()
# box_0.set_rgba([153 / 255, 183 / 255, 1, .3])
# box_0.set_pos(np.array([0, 0, .02]))
# box_0.attach_to(base)
# suction_s = suction.MVFLN40()
# loc_pos_box = np.array([.1, 0, .02])
# loc_rotmat_box = rm.rotmat_from_euler(math.pi, 0, 0)
# gl_pos_box = box_0.get_pos() + box_0.get_rotmat().dot(loc_pos_box)
# gl_rotmat_box = box_0.get_rotmat().dot(loc_rotmat_box)
# suction_s.suction_to_with_scpose(gl_pos_box, gl_rotmat_box)
# suction_s.gen_meshmodel(rgba=[.55, .55, .55, .3]).attach_to(base)
# gm.gen_stick(
# suction_s.pos,
# np.array([0,0,1]),rgba=[1,0,0,.3]).attach_to(base)
rtq_s = rtq.Robotiq85()
box = cm.gen_box(np.array([.3, .3, .04]))
box.set_rgba([153 / 255, 183 / 255, 1, 1])
box.set_pos(np.array([0, 0, .02]))
# box.set_rotmat(rm.rotmat_from_axangle([0, 1, 0], -math.pi / 12))
grasp_info_list = gpa.plan_grasps(
rtq_s,
box,
angle_between_contact_normals=math.radians(175),
openning_direction='loc_y')
# for grasp_info in grasp_info_list:
# jaw_width, gl_jaw_center_pos, gl_jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp_info
# rtq_s.fix_to(hnd_pos, hnd_rotmat)
# rtq_s.jaw_to(jaw_width)
# rtq_s.gen_meshmodel().attach_to(base)
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]['collision_model'] = 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]['collision_model'] = 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]['mesh_file'] = 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]['mesh_file'] = 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_jnt_id = -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_jnt_id = -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
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.gripper.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]),
# jaw_width=.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]),
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(13), 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.0, 0.0, 0.0])
self.lft_body.jnts[14]['loc_pos'] = np.array([.0, .258485281374, 1.61051471863])
self.lft_body.jnts[14]['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]['collision_model'] = 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]['collision_model'] = 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]['collision_model'] = 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]['collision_model'] = 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]['collision_model'] = 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]['collision_model'] = 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]['collision_model'] = 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]['collision_model'] = 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]['collision_model'] = 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]['collision_model'] = 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]['collision_model'] = 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]['collision_model'] = 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]['collision_model'] = cm.CollisionModel(
os.path.join(this_dir, "meshes", "ur3_dual_table1820x54x800.stl"))
self.lft_body.lnks[12]['rgba'] = [.9, .77, .52, 1.0]
# mounter
self.lft_body.lnks[13]['name'] = "ur3_dual_mounter"
self.lft_body.lnks[13]['loc_pos'] = np.array([0.0, 0.0, 1.439])
self.lft_body.lnks[13]['collision_model'] = cm.CollisionModel(
os.path.join(this_dir, "meshes", "mounter.stl"))
self.lft_body.lnks[13]['rgba'] = [.55, .55, .55, 1.0]
self.lft_body.reinitialize()
lft_arm_homeconf = np.zeros(6)
lft_arm_homeconf[0] = math.pi / 12.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]['collision_model'] = 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_gs.Robotiq85GelsightPusher(pos=self.lft_ft_sensor.jnts[-1]['gl_posq'],
# rotmat=self.lft_ft_sensor.jnts[-1]['gl_rotmatq'],
# enable_cc=False)
self.lft_hnd = rtq_gs.Robotiq85Gelsight(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]['mesh_file'] = 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 / 12.0
rgt_arm_homeconf[1] = -math.pi * 2.0 / 3.0
rgt_arm_homeconf[2] = math.pi * 2.0 / 3.0
lft_arm_homeconf[3] = math.pi
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]['collision_model'] = 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_jnt_id = -1
self.lft_arm.tcp_loc_rotmat = self.lft_ft_sensor.jnts[-1]['loc_rotmat'].dot(self.lft_hnd.jaw_center_rotmat)
self.lft_arm.tcp_loc_pos = self.lft_ft_sensor.jnts[-1]['loc_pos'] + self.lft_arm.tcp_loc_rotmat.dot(
self.lft_hnd.jaw_center_pos)
# rgt
self.rgt_arm.tcp_jnt_id = -1
self.rgt_arm.tcp_loc_rotmat = self.rgt_ft_sensor.jnts[-1]['loc_rotmat'].dot(self.rgt_hnd.jaw_center_rotmat)
self.rgt_arm.tcp_loc_pos = self.rgt_ft_sensor.jnts[-1]['loc_pos'] + self.rgt_arm.tcp_loc_rotmat.dot(
self.rgt_hnd.jaw_center_pos)
# 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 grasping.annotation.utils as gau
if __name__ == '__main__':
import numpy as np
import robot_sim.end_effectors.gripper.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()