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