def __init__(self, gripper, fn=None):
if not isinstance(gripper, GripperInfo):
raise ValueError("gripper needs to be a GripperInfo")
self.gripper = gripper
GripperInfo.register(gripper)
self.objects = []
self.object_to_grasps = dict()
if fn is not None:
self.load(fn)
def browse_database(gripper, dbfile=None):
from known_grippers import GripperInfo
g = GripperInfo.get(gripper)
if g is None:
print("Invalid gripper, valid names are",
list(GripperInfo.all_grippers.keys()))
exit(1)
db = GraspDatabase(g)
if dbfile is not None:
if dbfile.endswith('.json'):
db.load(dbfile)
else:
db.loadfolder(dbfile)
data_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), ''))
FIND_PATTERNS = [
data_dir + "/objects/%s.obj",
data_dir + "/objects/ycb-select/%s/textured.obj"
]
def _find_object(name):
for pat in FIND_PATTERNS:
fn = pat % (name, )
if os.path.exists(fn):
return fn
return None
w = WorldModel()
if not w.readFile(g.klampt_model):
print("Can't load gripper robot model", g.klampt_model)
exit(1)
robot = w.robot(0)
for o in db.objects:
fn = _find_object(o)
if fn is None:
print("Can't find object", o, "in usual paths...")
continue
if w.numRigidObjects() == 0:
obj = w.makeRigidObject(o)
if not obj.loadFile(fn):
if not obj.geometry().loadFile(fn):
print("Couldn't load object", o, "from", fn)
exit(1)
obj.setTransform(*se3.identity())
if len(db.objects) == 0:
print("Can't show anything, no objects")
print("Try adding some grasps to the database using grasp_edit.py")
exit(0)
data = dict()
data['cur_object'] = 0
data['cur_grasp'] = -1
data['shown_grasps'] = []
vis.add(db.objects[data['cur_object']], w.rigidObject(0))
def shift_object(amt, data=data):
vis.remove(db.objects[data['cur_object']])
data['cur_object'] += amt
if data['cur_object'] >= len(db.objects):
data['cur_object'] = 0
elif data['cur_object'] < 0:
data['cur_object'] = len(db.objects) - 1
if data['cur_object'] >= w.numRigidObjects():
for i in range(w.numRigidObjects(), data['cur_object'] + 1):
o = db.objects[i]
fn = _find_object(o)
obj = w.makeRigidObject(o)
if not obj.loadFile(fn):
if not obj.geometry().loadFile(fn):
print("Couldn't load object", o, "from", fn)
exit(1)
obj.setTransform(*se3.identity())
obj = w.rigidObject(data['cur_object'])
vis.add(db.objects[data['cur_object']], obj)
shift_grasp(None)
def shift_grasp(amt, data=data):
for i, grasp in data['shown_grasps']:
grasp.remove_from_vis("grasp" + str(i))
data['shown_grasps'] = []
all_grasps = db.object_to_grasps[db.objects[data['cur_object']]]
if amt == None:
data['cur_grasp'] = -1
else:
data['cur_grasp'] += amt
if data['cur_grasp'] >= len(all_grasps):
data['cur_grasp'] = -1
elif data['cur_grasp'] < -1:
data['cur_grasp'] = len(all_grasps) - 1
if data['cur_grasp'] == -1:
for i, grasp in enumerate(all_grasps):
grasp.ik_constraint.robot = robot
grasp.add_to_vis("grasp" + str(i))
data['shown_grasps'].append((i, grasp))
print("Showing", len(data['shown_grasps']), "grasps")
else:
grasp = all_grasps[data['cur_grasp']]
grasp.ik_constraint.robot = robot
grasp.add_to_vis("grasp" + str(data['cur_grasp']))
Tbase = grasp.ik_constraint.closestMatch(*se3.identity())
g.add_to_vis(robot, animate=False, base_xform=Tbase)
robot.setConfig(grasp.set_finger_config(robot.getConfig()))
data['shown_grasps'].append((data['cur_grasp'], grasp))
if grasp.score is not None:
vis.addText("score",
"Score %.3f" % (grasp.score, ),
position=(10, 10))
else:
vis.addText("score", "", position=(10, 10))
vis.addAction(lambda: shift_object(1), "Next object", '.')
vis.addAction(lambda: shift_object(-1), "Prev object", ',')
vis.addAction(lambda: shift_grasp(1), "Next grasp", '=')
vis.addAction(lambda: shift_grasp(-1), "Prev grasp", '-')
vis.addAction(lambda: shift_grasp(None), "All grasps", '0')
vis.add("gripper", w.robot(0))
vis.run()
"""Registers all known grippers.
If you run this as the main file with no arguments, it will display all the
known grippers.
If you run this with one or more arguments, it will display the named grippers.
"""
from gripper import GripperInfo
import os
data_dir = os.path.abspath(os.path.join(os.path.dirname(__file__),''))
robotiq_85 = GripperInfo("robotiq_85",0,[1,2,3,4,5,6,7,8],[0],
klampt_model=os.path.join(data_dir,"robot/robotiq_85.rob"))
robotiq_85.center = (0,0,0.1)
robotiq_85.primary_axis = (0,0,1)
robotiq_85.secondary_axis = (1,0,0)
robotiq_85.finger_length = 0.06
robotiq_85.finger_depth = 0.01
robotiq_85.finger_width = 0.02
robotiq_85.maximum_span = 0.085 - 0.01
robotiq_85.minimum_span = 0
robotiq_85.open_config = [0]*8
robotiq_85.closed_config = [0.723,0,0.723,-0.723,-0.723,0.723,-0.723,0 ]
robotiq_85_kinova_gen3 = GripperInfo.mounted(robotiq_85,os.path.join(data_dir,"robot/kinova_with_robotiq_85.urdf"),"gripper:Link_0","robotiq_85-kinova_gen3")
if __name__ == '__main__':
from klampt import vis
import sys
if len(sys.argv) == 1:
grippers = [i for i in GripperInfo.all_grippers]
"""Registers all known grippers.
If you run this as the main file with no arguments, it will display all the
known grippers.
If you run this with one or more arguments, it will display the named grippers.
"""
from gripper import GripperInfo
import os
data_dir = os.path.abspath(os.path.join(os.path.dirname(__file__),'../data'))
robotiq_85 = GripperInfo("robotiq_85",0,[1,2,3,4,5,6,7,8],[0],
klampt_model=os.path.join(data_dir,"robots/robotiq_85.rob"))
robotiq_85.center = (0,0,0.1)
robotiq_85.primary_axis = (0,0,1)
robotiq_85.secondary_axis = (1,0,0)
robotiq_85.finger_length = 0.06
robotiq_85.finger_depth = 0.01
robotiq_85.finger_width = 0.02
robotiq_85.maximum_span = 0.085 - 0.01
robotiq_85.minimum_span = 0
robotiq_85.open_config = [0]*8
robotiq_85.closed_config = [0.723,0,0.723,-0.723,-0.723,0.723,-0.723,0 ]
robotiq_140 = GripperInfo("robotiq_140",0,[1,2,4,6,7,9],[0],
klampt_model=os.path.join(data_dir,"robots/robotiq_140.rob"))
robotiq_140.center = (0,0,0.1)
robotiq_140.primary_axis = (0,0,1)
robotiq_140.secondary_axis = (0,1,0)
robotiq_140.finger_length = 0.12
grasps = self._object_to_grasps[self._matching_object]
if self._grasp_index >= len(grasps):
return 0
return 1.0 - self._grasp_index/float(len(grasps))
if __name__ == '__main__':
import sys
from klampt import WorldModel
from klampt import vis
if len(sys.argv) < 2:
print("Usage: python grasp_database.py gripper [FILE]")
exit(0)
from known_grippers import *
print(sys.argv)
g = GripperInfo.get(sys.argv[1])
if g is None:
print("Invalid gripper, valid names are",list(GripperInfo.all_grippers.keys()))
exit(1)
db = GraspDatabase(g)
if len(sys.argv) >= 3:
if sys.argv[2].endswith('.json'):
db.load(sys.argv[2])
else:
db.loadfolder(sys.argv[2])
data_dir = os.path.abspath(os.path.join(os.path.dirname(__file__),'../data'))
FIND_PATTERNS = [data_dir+"/objects/%s.obj",data_dir+"/objects/ycb-select/%s/textured.obj"]
def _find_object(name):
for pat in FIND_PATTERNS:
fn = pat%(name,)