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