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