def editExistingGrasp(item,object,gindex): global world,robot grasp = None if isinstance(gindex,int): grasp = item.grasps[gindex] else: for g in item.grasps: if g.name == gindex: grasp = g break if grasp == None: print "Grasp",gindex,"does not exist, creating new grasp" raise RuntimeError("Invalid grasp") #get the object transform object.setTransform(*se3.inv(grasp.grasp_xform)) open_config = grippermodule.commandToConfig(grasp.gripper_open_command) closed_config = grippermodule.commandToConfig(grasp.gripper_close_command) robot.setConfig(closed_config) xform = resource.get(name=None,type='RigidTransform',default=object.getTransform(),description="Transform of "+item.name,world=world,frame=object) if xform==None: return None #set the object in the world object.setTransform(*xform) #get the hand open configuration open_config = resource.get(name=None,type='Config',default=open_config,description="Hand open configuration",world=world) if open_config==None: return None closed_config = resource.get(name=None,type='Config',default=closed_config,description="Hand closed configuration",world=world) if closed_config==None: return None #return grasp grasp.grasp_xform = se3.inv(xform) grasp.gripper_close_command = grippermodule.configToCommand(closed_config) grasp.gripper_open_command = grippermodule.configToCommand(open_config) return grasp
def editNewGrasp(item,object,name=None,openClose=1): global world,robot global default_open_config,default_close_config #get the object transform xform = resource.get(name=None, type='RigidTransform', default=object.getTransform(), description="Transform of "+item.name, world=world,frame=object) if xform==None: return None #set the object in the world object.setTransform(*xform) if openClose: #get the hand open configuration open_config = resource.get(name=None,type='Config',default=default_open_config,description="Hand open configuration",world=world) if open_config==None: return None closed_config = resource.get(name=None,type='Config',default=open_config,description="Hand closed configuration",world=world) if closed_config==None: return None #return grasp grasp = apc.ItemGrasp() grasp.grasp_xform = se3.inv(xform) if openClose: grasp.gripper_close_command = grippermodule.configToCommand(closed_config) grasp.gripper_open_command = grippermodule.configToCommand(open_config) if name==None: #default: just number it grasp.name = str(len(item.grasps)) else: grasp.name = name return grasp
def __init__(self, configFile, viconFile): self._viconFile = viconFile self._configFile = configFile self._viconMarkers = utils.parseCSVToMarkers(viconFile) configfn = configFile.split('/')[-1] configDir = '/'.join(configFile.split('/')[:-1]) self._gloveConfigs = resource.get(configfn, directory=configDir) self.data = self._makeData()
def localizeOrderBin(self): order_bin_xform = (so3.identity(),[0.57,0,-0.87]) resource.setDirectory("resources/") return resource.get("calibrated_order_bin.xform",type="RigidTransform",default=order_bin_xform,doedit=False) T = numpy.array([[ 1, 0, 0, 0.57 ], [ 0, 1, 0, 0 ], [ 0, 0, 1, -0.87 ], [ 0, 0, 0, 1 ]]) return (list(T[:3, :3].T.flat), list(T[:3, 3].flat))
def localizeShelf(self): #distance away shelf_xform = ([0,1,0,-1,0,0,0,0,1],[1.1,-0.055,-.9]) resource.setDirectory("resources/") return resource.get("calibrated_shelf.xform",type="RigidTransform",default=shelf_xform,doedit=False) T = numpy.array([[ 0, -1, 0, 1.1 ], # distance from front of robot [ 1, 0, 0, -0.055 ], # left/right [ 0, 0, 1, -0.9 ], # height [ 0, 0, 0, 1 ]]) return (list(T[:3, :3].T.flat), list(T[:3, 3].flat))
left_arm_geometry_indices = [15,16,17,18,19,21,22,23,30,31] right_arm_geometry_indices = [35,36,37,38,39,41,42,43,50,51] left_hand_link_start = 54 right_hand_link_start = 54+gripper.numDofs left_hand_geometry_indices = range(54,54+gripper.numDofs) right_hand_geometry_indices = range(54+gripper.numDofs,54+gripper.numDofs*2) #indices of the left and right arms in the Baxter robot file left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist'] right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist'] #local transformations (rotation, translation pairs) of the grasp center resource.setDirectory("resources/reflex") left_gripper_center_xform = resource.get("left_gripper_center.xform", type='RigidTransform', description="Left gripper center", world="klampt_models/"+klampt_model_name, frame=left_gripper_link_name) right_gripper_center_xform = resource.get("right_gripper_center.xform", type='RigidTransform', default=left_gripper_center_xform, description="Right gripper center", world="klampt_models/"+klampt_model_name, frame=right_gripper_link_name) #resting configuration rest_config = resource.get("baxter_rest.config",type="Config",description="Rest configuration",world="klampt_models/"+klampt_model_name) def set_model_gripper_command(robot,limb,command): """Given the Baxter RobotModel 'robot' at its current configuration, this will set the configuration so the gripper on limb 'limb' is
world = WorldModel() world.readFile('../ece490-s2016/apc2015/klampt_models/apc.xml') #sets the world to the state depicted in the file obj = ik.objective(world.robot(0).link(22),local=[0,0,0],world=[1,0,1]) visualization.add("world",world) #visualization.add("ik objective",obj) q0 = world.robot(0).getConfig() #copy vector of world robot config q1 = q0[:] #q1[16] = 1 #q2 = q1[:] #q2[16] = 0 #path = [q0,q1,q2] q1 = resource.get("goal.config",description="Goal config for left arm",doedit=True,default=q1,editor='visual',world=world) #robot links under suspicion? subset = [15,16,17,18,19,20,21,22] collider = robotcollide.WorldCollider(world) #probably want to ignore collisions beteween other arm/links and the world to make things faster... space = robotcspace.RobotSubsetCSpace(world.robot(0),subset,collider) planner = cspace.MotionPlan(space, "rrt*") #extract out cspace configurations start = [q0[i] for i in subset] goal = [q1[i] for i in subset] print "Goal config",goal planner.setEndpoints(start,goal) for iters in xrange(10000): planner.planMore(1)
qmin,qmax = robot.getJointLimits() numObs = 10 jointEncoderError = 1e-5 sensorErrorRads = 1e-2 sensorErrorMeters = 2e-3 trueCalibrationConfigs = [] calibrationConfigs = [] trueObservations = [] observations = [] for obs in xrange(numObs): q0 = [random.uniform(a,b) for (a,b) in zip(qmin,qmax)] #don't move head for i in range(13): q0[i] = 0 trueCalibrationConfigs.append(q0) trueCalibrationConfigs=resource.get("calibration.configs",default=trueCalibrationConfigs,type="Configs",description="Calibration configurations",world=world) for q0 in trueCalibrationConfigs: robot.setConfig(q0) obs0 = se3.mul(se3.inv(lc.getTransform()),lm.getTransform()) dq = [random.uniform(-jointEncoderError,jointEncoderError) for i in range(len(q0))] dobs = (so3.from_moment([random.uniform(-sensorErrorRads,sensorErrorRads) for i in range(3)]),[random.uniform(-sensorErrorMeters,sensorErrorMeters) for i in range(3)]) calibrationConfigs.append(vectorops.add(q0,dq)) observations.append(se3.mul(obs0,dobs)) trueObservations.append(obs0) if DO_VISUALIZATION: rgroup = coordinates.addGroup("calibration ground truth") rgroup.addFrame("camera link",worldCoordinates=pc.getTransform()) rgroup.addFrame("marker link",worldCoordinates=pm.getTransform()) rgroup.addFrame("camera (ground truth)",parent="camera link",relativeCoordinates=Tc0) rgroup.addFrame("marker (ground truth)",parent="marker link",relativeCoordinates=Tm0)
#load the items and spawn one in the world apc.load_item(objectname,gripper=grippername) item = apc.item_info[objectname] item.geometry = apc.load_item_geometry(item) itemposed = apc.ItemInBin(item,'blah') itemposed.xform = se3.identity() apc.spawn_item_in_world(itemposed,world) #set the directory print "Resouce directory is",os.path.join('../resources/',os.path.splitext(grippermodule.klampt_model_name)[0]) resource.setDirectory(os.path.join('../resources/',os.path.splitext(grippermodule.klampt_model_name)[0])) #edit the default configurations default_open_config = resource.get('default_open.config', description='Default hand open configuration', doedit='auto', world=world) default_closed_config = resource.get('default_closed.config', description='Default hand closed configuration', doedit='auto', world=world) def editNewGrasp(item,object,name=None,openClose=1): global world,robot global default_open_config,default_close_config #get the object transform xform = resource.get(name=None, type='RigidTransform', default=object.getTransform(), description="Transform of "+item.name,
print """resourcetest.py: This program gives an example of how to use the resource module.""" worldfile = "../../data/athlete_plane.xml" robotname = 'athlete' world = WorldModel() world.readFile(worldfile) #look in resources/athlete/ resource.setDirectory('resources/' + robotname) config = resource.get("resourcetest1.config", description="First config, always edited", doedit=True, editor='visual', world=world) print "Config 1:", config config = resource.get("resourcetest1.config", description="Trying this again...", editor='visual', world=world) print "Config 2:", config config = resource.get("resourcetest2.config", description="Another configuration", editor='visual', world=world) print "Config 3:", config
from klampt import resource from klampt import * print """resourcetest.py: This program gives an example of how to use the resource module.""" worldfile = "../../data/athlete_plane.xml" robotname = 'athlete' world = WorldModel() world.readFile(worldfile) #look in resources/athlete/ resource.setDirectory('resources/'+robotname) config = resource.get("resourcetest1.config",description="First config, always edited",doedit=True,editor='visual',world=world) print "Config 1:",config config = resource.get("resourcetest1.config",description="Trying this again...",editor='visual',world=world) print "Config 2:",config config = resource.get("resourcetest2.config",description="Another configuration",editor='visual',world=world) print "Config 3:",config if config != None: config[3] += 1.0 resource.set("resourcetest3_high.config",config) world.robot(0).setConfig(config) #testing transform editor xform = resource.get(name=None,type='RigidTransform',frame=world.robot(0).getLink(5).getTransform(),world=world)
q = world.robot(0).getConfig() q[9] = -1.0 world.robot(0).setConfig(q) visualization.unlock() time.sleep(1.0) if not visualization.shown(): break print "Hiding visualization window" visualization.show(False) """ # look in resources/athlete/ resource.setDirectory("resources/" + robotname) config1 = resource.get( "resourcetest1.config", description="First config, always edited", doedit=True, editor="visual", world=world ) print "Config 1:", config1 config2 = resource.get("resourcetest1.config", description="Trying this again...", editor="visual", world=world) print "Config 2:", config2 config3 = resource.get("resourcetest2.config", description="Another configuration", editor="visual", world=world) print "Config 3:", config3 if config3 != None: config3hi = config3[:] config3hi[3] += 1.0 resource.set("resourcetest3_high.config", config3hi) world.robot(0).setConfig(config3hi) configs = []
left_arm_link_names = [ 'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow', 'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm', 'left_wrist' ] right_arm_link_names = [ 'right_upper_shoulder', 'right_lower_shoulder', 'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm', 'right_lower_forearm', 'right_wrist' ] #local transformations (rotation, translation pairs) of the grasp center resource.setDirectory("resources/reflex") left_gripper_center_xform = resource.get("left_gripper_center.xform", type='RigidTransform', description="Left gripper center", world="klampt_models/" + klampt_model_name, frame=left_gripper_link_name) right_gripper_center_xform = resource.get("right_gripper_center.xform", type='RigidTransform', default=left_gripper_center_xform, description="Right gripper center", world="klampt_models/" + klampt_model_name, frame=right_gripper_link_name) #resting configuration rest_config = resource.get("baxter_rest.config", type="Config", description="Rest configuration", world="klampt_models/" + klampt_model_name)