def checkgrasp(self): grasp = gpa.load_pickle_file(self.objname, None, "rtqhe.pickle") grasp_info_list = grasp print("number of grasp 1", len(grasp)) hndfa = rtqhe.RobotiqHE(enable_cc=True) rtqHE = copy.deepcopy(hndfa) rtqHE.jaw_to(jawwidth=0.035) slope = self.slopelist_high obj = cm.CollisionModel(initor=self.objpath) obj.set_scale((0.001, 0.001, 0.001)) cfreegrasp = [] for i, rotmat4 in enumerate(self.pRotMat): temcfreegrasp = [] if self.collisionlist[i] == False and self.stablelist[i] == True: obstaclelist = slope obj.set_homomat(da.pdmat4_to_npmat4(rotmat4)) obstaclelist.append(obj) for grasp in grasp_info_list: jaw_width, jaw_center_pos, jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp rotmatgraspnp = np.dot( da.pdmat4_to_npmat4(rotmat4), rm.homomat_from_posrot(hnd_pos, hnd_rotmat)) rotmat4panda = da.npmat4_to_pdmat4(rotmatgraspnp) rtqHE.fix_to(pos=rotmatgraspnp[:3, 3], rotmat=rotmatgraspnp[:3, :3], jawwidth=jaw_width) if rtqHE.is_mesh_collided(obstaclelist) == False: temcfreegrasp.append([rotmat4panda, False]) else: temcfreegrasp.append([rotmat4panda, True]) print("grasp in collision") cfreegrasp.append(temcfreegrasp) print("number of grasp 2", len(temcfreegrasp)) self.cfreegrasppose = cfreegrasp
def show_ikfeasible_poses(obj_rotmat, obj_pos): hndfa = rtqhe.RobotiqHE(enable_cc=True) obj_fixture = object.copy() obj_fixture.set_rotmat(obj_rotmat) obj_fixture.set_pos(obj_pos) obj_fixture.attach_to(base) for i, item in enumerate(grasp_info_list): jaw_width, gl_jaw_center_pos, gl_jaw_center_rotmat, hnd_pos, hnd_rotmat = item hnd_rotmat = obj_rotmat.dot(gl_jaw_center_rotmat) hnd_pos = obj_rotmat.dot(gl_jaw_center_pos) + obj_pos hndfa.fix_to(pos=hnd_pos, rotmat=hnd_rotmat, jawwidth=.04) # hndfa.gen_meshmodel(rgba=(1, 0, 0, 0.05)).attach_to(base) jnt_values = robot.ik(component_name=component_name, tgt_pos=hnd_pos, tgt_rotmat=hnd_rotmat, max_niter=500, toggle_debug=False, seed_jnt_values=None) if jnt_values is not None: robot.fk(component_name=component_name, jnt_values=jnt_values) if hndfa.is_mesh_collided(slopeforcd_high, toggle_debug=False): robot.gen_meshmodel(toggle_tcpcs=False, rgba=(1, 0, 0, 0.05)).attach_to(base) # pass else: robot.gen_meshmodel(toggle_tcpcs=False, rgba=(0, 1, 0, 0.5)).attach_to(base) print(f"IK Done!, feasible grasp ID {i}")
import numpy as np import basis.robot_math as rm import visualization.panda.world as wd import robot_sim.end_effectors.grippers.robotiqhe.robotiqhe as rtq_he import modeling.geometric_model as gm if __name__ == "__main__": base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, 0]) gm.gen_frame(length=.2).attach_to(base) grpr = rtq_he.RobotiqHE(enable_cc=True) grpr.jaw_to(.05) grpr.gen_meshmodel(rgba=[.3,.3,.3,.3]).attach_to(base) grpr.gen_stickmodel(toggle_tcpcs=True, toggle_jntscs=True).attach_to(base) grpr.fix_to(pos=np.array([-.1, .2, 0]), rotmat=rm.rotmat_from_axangle([1, 0, 0], .05)) grpr.gen_meshmodel().attach_to(base) grpr.show_cdmesh() grpr.fix_to(pos=np.array([.1, -.2, 0]), rotmat=rm.rotmat_from_axangle([1, 0, 0], .05)) grpr.gen_meshmodel().attach_to(base) grpr.show_cdprimit() base.run()
def update(objmnp, counter, testnode, normalnode, graspnode, textNPose, textNPose_lp, picnode, task): if base.inputmgr.keymap['space'] is True: hndfa = rtqhe.RobotiqHE(enable_cc=True) if counter[0] < len(self.pRotMat): print("It's No.", counter[0], "placement") print("Is it collision?", self.collisionlist[counter[0]]) if objmnp[0] is not None: objmnp[0].detach() normalnode[0].detachNode() graspnode[0].detachNode() textNPose[0].detachNode() textNPose[1].detachNode() # textNPose_lp[0].detachNode() objmnp[0] = cm.CollisionModel(initor=self.objpath) objmnp[0].set_scale((0.001, 0.001, 0.001)) objmnp[0].set_homomat( da.pdmat4_to_npmat4(self.pRotMat[counter[0]])) normalnode[0] = NodePath("normal") graspnode[0] = NodePath("graspnode") textNPose[0] = NodePath("text") textNPose[1] = NodePath("text") textNPose_lp[0] = NodePath("text") # graspnode[0].reparentTo(base.render) normalnode[0].reparentTo(base.render) for j in range(len(self.cfreegrasppose[counter[0]])): if j % 30 != 0: if not self.cfreegrasppose[counter[0]][j][1]: hndfa.gen_meshmodel(rgba=(0, 1, 0, 0.15)).attach_to( graspnode[0]) else: continue print("number of grasp 3", len(self.cfreegrasppose[counter[0]])) hndfa.fix_to( pos=da.pdmat4_to_npmat4( self.cfreegrasppose[counter[0]][j][0])[:3, 3], rotmat=da.pdmat4_to_npmat4( self.cfreegrasppose[counter[0]][j][0])[:3, :3]) if self.cfreegrasppose[counter[0]][j][1]: hndfa.gen_meshmodel(rgba=(1, 0, 0, 0.05)).attach_to( graspnode[0]) else: hndfa.gen_meshmodel(rgba=(0, 1, 0, 0.15)).attach_to( graspnode[0]) graspnode[0].reparentTo(base.render) self.showfeatures( node=normalnode[0], normal=self.pNormals[counter[0]], placementfacetpairscenter=self.pFacetpairscenter[ counter[0]], placementVertices=self.pVertices[counter[0]], placementCoM=self.pCoM[counter[0]], showCoM=True, showVertices=False, showNormal=False) if self.stablelist is not None: print("stable?", self.stablelist[counter[0]]) # textNPose[0] = OnscreenText( # text="Stability (uniform) is:"+str(self.stabilityindex[0][counter[0]]), pos=(-.9, -.8, 0), # scale=0.1, # fg=(1., 0, 0, 1), # align=TextNode.ALeft, mayChange=1) # textNPose_lp[0] = OnscreenText( # text="Stability (lp u=0.3) is:" + str(self.stabilityindex[1][counter[0]]), pos=(-.9, -.9, 0), # scale=0.1, # fg=(1., 0, 0, 1), # align=TextNode.ALeft, mayChange=1) # checker = cdchecker.MCMchecker(toggledebug=True) # checker.showMesh(objmnp[0]) if self.collisionlist[counter[0]]: objmnp[0].set_rgba((1, 0, 0, 1)) # objmnp[0].set_rgba((0.1, 0.1, 0.1, 1)) else: if self.stablelist[counter[0]]: objmnp[0].set_rgba((0, 191 / 255, 1, 1)) objmnp[0].set_rgba((0.1, 0.1, 0.1, 0.5)) else: objmnp[0].set_rgba((0.1, 0.1, 0.1, 1)) objmnp[0].attach_to(base) counter[0] += 1 else: counter[0] = 0 if base.inputmgr.keymap['w'] is True: if counter[0] < len(self.pRotMat): print("It's No.", counter[0], "placement") print("Is it collision?", self.collisionlist[counter[0]]) if objmnp[0] is not None: objmnp[0].detach() normalnode[0].detachNode() graspnode[0].detachNode() textNPose[0].detachNode() textNPose[1].detachNode() # picnode[0].detachNode() # picnode[1].detachNode() objmnp[0] = cm.CollisionModel(initor=self.objpath) objmnp[0].set_scale((0.001, 0.001, 0.001)) objmnp[0].set_homomat( da.pdmat4_to_npmat4(self.pRotMat[counter[0]])) normalnode[0] = NodePath("normal") graspnode[0] = NodePath("graspnode") textNPose[0] = NodePath("text") textNPose[1] = NodePath("text") graspnode[0].reparentTo(base.render) normalnode[0].reparentTo(base.render) # picnode[0] = NodePath("obj") # picnode[0].setMat(da.npmat4_to_pdmat4(rm.homomat_from_posrot(rot=rm.rotmat_from_axangle([1, 0, 0], np.radians(-45))))) # picnode[0].reparentTo(base.render) # picnode[1] = OnscreenImage(picnode[0], pos=(150, 0, 0), scale=100, parent=testnode[0]) # image = self.picnodepath[counter[0]] # picnode[1].setImage(image=image) textNPose[1] = OnscreenText(text="ID" + str([counter[0]]), pos=(-.9, -.6, 0), scale=0.1, fg=(0, 0, 0, 1), align=TextNode.ALeft, mayChange=1) # for j in range(len(self.cfreegrasppose[counter[0]])): # rtqHE = hndfa.genHand() # rtqHE.setMat(self.cfreegrasppose[counter[0]][j]) # rtqHE.reparentTo(graspnode[0]) # self.showsingleNormal(node = normalnode[0], normal = self.placementNormals[counter[0]], # placementfacetpairscenter = self.placementfacetpairscenter[counter[0]], # placementVertices = self.placementVertices[counter[0]], # placementCoM = self.placementCoM[counter[0]]) self.showfeatures( node=normalnode[0], normal=self.pNormals[counter[0]], placementfacetpairscenter=self.pFacetpairscenter[ counter[0]], placementVertices=self.pVertices[counter[0]], placementCoM=self.pCoM[counter[0]], showCoM=True, showVertices=False, showNormal=False) if self.stablelist is not None: print("stable?", self.stablelist[counter[0]]) textNPose[0] = OnscreenText( text="Stability (uniform) is:" + str(self.stabilityindex_nolp[counter[0]]), pos=(-.9, -.8, 0), scale=0.1, fg=(1., 0, 0, 1), align=TextNode.ALeft, mayChange=1) # textNPose_lp[0] = OnscreenText( # text="Stability (lp u=0.3) is:" + str(self.stabilityindex[1][counter[0]]), pos=(-.9, -.9, 0), # scale=0.1, # fg=(1., 0, 0, 1), # align=TextNode.ALeft, mayChange=1) # checker = cdchecker.MCMchecker(toggledebug=True) # checker.showMesh(objmnp[0]) if self.collisionlist[counter[0]]: objmnp[0].set_rgba((1, 0, 0, 0.5)) else: if self.stablelist is not None: if self.stablelist[counter[0]]: objmnp[0].set_rgba((0, 191 / 255, 1, 0.5)) objmnp[0].set_rgba((0.1, 0.1, 0.1, 0.5)) # objmnp[0].set_rgba((1, 0, 0, 1)) else: objmnp[0].set_rgba((0.1, 0.1, 0.1, 0.5)) # objmnp[0].set_rgba((1, 0, 0, 1)) else: objmnp[0].set_rgba((0, 191 / 255, 1, 0.5)) objmnp[0].set_rgba((0.1, 0.1, 0.1, 0.5)) # objmnp[0].set_rgba((1, 0, 0, 1)) objmnp[0].attach_to(base) counter[0] += 1 else: counter[0] = 0 return task.again
import modeling.geometric_model as gm import modeling.collision_model as cm import grasping.planning.antipodal as gpa import robot_sim.end_effectors.grippers.yumi_gripper.yumi_gripper as yg import robot_sim.end_effectors.grippers.robotiqhe.robotiqhe as rtqhe base = wd.World(cam_pos=[1, 1, 1],w=960, h=540, lookat_pos=[0, 0, 0]) gm.gen_frame().attach_to(base) # object object_tube = cm.CollisionModel("objects/test_long.STL") object_tube.set_rgba([.9, .75, .35, 1]) object_tube.set_scale([0.001, 0.001, 0.001]) object_tube.attach_to(base) # hnd_s gripper_s = rtqhe.RobotiqHE() grasp_info_list = gpa.plan_grasps(gripper_s, object_tube, angle_between_contact_normals=math.radians(177), openning_direction='loc_x', rotation_interval=math.radians(22.5/2), max_samples=30, min_dist_between_sampled_contact_points=.01, contact_offset=.001) gpa.write_pickle_file('test_long', grasp_info_list, './', 'rtqhe.pickle') for grasp_info in grasp_info_list: jaw_width, jaw_center_pos, jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp_info gripper_s.grip_at_with_jcpose(jaw_center_pos, jaw_center_rotmat, jaw_width) gripper_s.gen_meshmodel(rgba=(0,1,0,0.1)).attach_to(base) # jaw_width, jaw_center_pos, jaw_center_rotmat, hnd_pos, hnd_rotmat base.run()
import robot_sim.end_effectors.grippers.robotiqhe.robotiqhe as hnde base = wd.World(cam_pos=[1, 1, 1], w=960, h=540, lookat_pos=[0, 0, 0]) gm.gen_frame().attach_to(base) objname = "tubebig" grippername = "hnde" # object object_tube = cm.CollisionModel(f"objects/{objname}.stl") object_tube.set_rgba([.9, .75, .35, 1]) object_tube.attach_to(base) # hnd_s # gripper_s = yg.YumiGripper() gripper_s = hnde.RobotiqHE() grasp_info_list = gpa.plan_grasps( gripper_s, object_tube, angle_between_contact_normals=math.radians(177), openning_direction='loc_x', max_samples=15, min_dist_between_sampled_contact_points=.005, contact_offset=.005) gpa.write_pickle_file('tubebig', grasp_info_list, '.', f'graspdata/{grippername}_{objname}.pickle') for grasp_info in grasp_info_list: jaw_width, jaw_center_pos, jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp_info gripper_s.grip_at_with_jcpose(jaw_center_pos, jaw_center_rotmat, jaw_width) gripper_s.gen_meshmodel(rgba=(1, 0, 0, 0.01)).attach_to(base) base.run()