class GLTransferPlanPlugin(GLPluginInterface): def __init__(self, world): GLPluginInterface.__init__(self) self.world = world self.robot = world.robot(0) self.object = world.rigidObject(0) #start robot config self.qstart = self.robot.getConfig() #start object transform self.Tstart = self.object.getTransform() #grasp transform self.Tgrasp = Tgrasp = graspTransform(self.robot, Hand('l'), self.qstart, self.Tstart) #solution to planning problem self.path = None def keyboardfunc(self, key, x, y): if key == 'r': self.robot.setConfig(self.qstart) self.object.setTransform(*self.Tstart) self.path = planTransfer(self.world, 0, Hand('l'), (0, -0.15, 0)) if self.path: #convert to a timed path for animation's sake self.path = RobotTrajectory(self.robot, list(range(len(self.path))), self.path) #compute object trajectory resolution = 0.05 self.objectPath = self.path.getLinkTrajectory( Hand('l').link, resolution) self.objectPath.postTransform(self.Tgrasp) else: self.path = None #reset the animation vis.animate(("world", self.robot.getName()), self.path) vis.animate(("world", self.object.getName()), self.objectPath) return True elif key == 'f': self.robot.setConfig(self.qstart) self.object.setTransform(*self.Tstart) self.path = planTransfer(self.world, 0, Hand('l'), (0.15, 0, 0)) if self.path: #convert to a timed path for animation's sake self.path = RobotTrajectory(self.robot, list(range(len(self.path))), self.path) #compute object trajectory resolution = 0.05 self.objectPath = self.path.getLinkTrajectory( Hand('l').link, resolution) self.objectPath.postTransform(self.Tgrasp) else: self.path = None #reset the animation vis.animate(("world", self.robot.getName()), self.path) vis.animate(("world", self.object.getName()), self.objectPath) return True return False
""" xtraj = trajcache.trajectoryToState(trajsolved) ctest2.setx(xtraj) print("Resulting constraint residual",ctest2.minvalue(xtraj)) ctest2.clearx() raw_input("Press enter to continue... ") """ timescale = 10 trajinit.times = [timescale * v for v in trajinit.times] if trajinit is not trajsolved: trajsolved.times = [timescale * v for v in trajsolved.times] vis.add( "Initial trajectory", trajinit.getLinkTrajectory( END_EFFECTOR_LINK, 0.1).getPositionTrajectory(END_EFFECTOR_LOCAL_POSITION)) #vis.add("Initial trajectory",trajinit) vis.setColor("Initial trajectory", 1, 1, 0, 0.5) vis.hideLabel("Initial trajectory") vis.setAttribute("Initial trajectory", "width", 2) eetrajopt = trajsolved.getLinkTrajectory( END_EFFECTOR_LINK, 0.1).getPositionTrajectory(END_EFFECTOR_LOCAL_POSITION) vis.add("Solution trajectory", eetrajopt) #vis.add("Solution trajectory",trajsolved) vis.hideLabel("Solution trajectory") vis.animate(("world", robot.getName()), trajsolved) params[0] = sorted(params[0], key=lambda x: x[2]) for i, p in enumerate(params[0]): link = p[0]
def keyboardfunc(self, key, x, y): h = 0.932 targets = { 'a': (0.35, 0.25, h), 'b': (0.35, 0.05, h), 'c': (0.35, -0.1, h), 'd': (0.35, -0.1, h) } if key in targets: dest = targets[key] shift = vectorops.sub(dest, self.Tstart[1]) self.robot.setConfig(self.qstart) self.object.setTransform(*self.Tstart) self.hand = Hand('l') #plan transit path to grasp object self.transitPath = planTransit(self.world, 0, self.hand) if self.transitPath: #plan transfer path self.Tgrasp = graspTransform(self.robot, self.hand, self.transitPath[-1], self.Tstart) self.robot.setConfig(self.transitPath[-1]) self.transferPath = planTransfer(self.world, 0, self.hand, shift) if self.transferPath: self.Tgoal = graspedObjectTransform( self.robot, self.hand, self.transferPath[0], self.Tstart, self.transferPath[-1]) #plan free path to retract arm self.robot.setConfig(self.transferPath[-1]) self.object.setTransform(*self.Tgoal) self.retractPath = planFree(self.world, self.hand, self.qstart) #reset the animation if self.transitPath and self.transferPath and self.retractPath: milestones = self.transitPath + self.transferPath + self.retractPath self.path = RobotTrajectory(self.robot, range(len(milestones)), milestones) resolution = 0.05 xfertraj = RobotTrajectory(self.robot, range(len(self.transferPath)), self.transferPath) xferobj = xfertraj.getLinkTrajectory(self.hand.link, resolution) xferobj.postTransform(self.Tgrasp) #offset times to be just during the transfer stage for i in xrange(len(xferobj.times)): xferobj.times[i] += len(self.transitPath) self.objectPath = xferobj vis.animate(("world", self.robot.getName()), self.path) vis.animate(("world", self.object.getName()), self.objectPath) else: vis.animate(("world", self.robot.getName()), None) vis.animate(("world", self.object.getName()), None) if key == 'n': print "Moving to next action" if self.transitPath and self.transferPath and self.retractPath: #update start state self.qstart = self.retractPath[-1] self.Tstart = self.Tgoal self.robot.setConfig(self.qstart) self.object.setTransform(*self.Tstart) self.transitPath = None self.transferPath = None self.hand = None self.Tgrasp = None self.refresh()