def moveRobot(robot, values):
basemanip = BaseManipulation(robot)
with robot:
robot.SetActiveDOFs(arange(robot.GetDOF()))
basemanip.MoveActiveJoints(values)
while not robot.GetController().IsDone():
time.sleep(0.01)
def __init__(self,robot,grmodel=None,switchpatterns=None,maxvelmult=None):
self.env=robot.GetEnv()
self.envreal = None
self.robot = robot
self.grmodel = grmodel
self.maxvelmult=maxvelmult
self.basemanip = BaseManipulation(self.robot,maxvelmult=maxvelmult)
self.taskmanip = TaskManipulation(self.robot,maxvelmult=maxvelmult)
self.switchpatterns=switchpatterns
if switchpatterns is not None:
self.taskmanip.SwitchModels(switchpatterns=switchpatterns)
self.updir = array((0,0,1))
def testSampling(self,delay=0,**kwargs):
"""random sample goals on the current environment"""
with self.env:
configsampler = self.sampleValidPlacementIterator(**kwargs)
basemanip = BaseManipulation(self.robot,maxvelmult=self.maxvelmult)
taskmanip = TaskManipulation(self.robot,maxvelmult=self.maxvelmult)
jointvalues = self.robot.GetDOFValues()
with RobotStateSaver(self.robot):
while True:
try:
with self.env:
self.robot.SetDOFValues(jointvalues) # reset to original
pose,values,grasp,graspindex = next(configsampler)
gmodel = graspindex[0]
print('found grasp',gmodel.target.GetName(),graspindex[1])
self.robot.SetActiveManipulator(gmodel.manip)
self.robot.SetTransform(pose)
gmodel.setPreshape(grasp)
self.robot.SetDOFValues(values)
final,traj = taskmanip.CloseFingers(execute=False,outputfinal=True)
self.robot.SetDOFValues(final,gmodel.manip.GetGripperIndices())
self.env.UpdatePublishedBodies()
if delay > 0:
time.sleep(delay)
except planning_error as e:
traceback.print_exc(e)
continue
def __init__(self, env):
self.env = env
self.env.SetDebugLevel(DebugLevel.Verbose)
self.collision_checker = self.env.CreateCollisionChecker('pqp')
self.collision_checker.SetCollisionOptions(CollisionOptions.Distance
| CollisionOptions.Contacts)
self.env.SetCollisionChecker(self.collision_checker)
self.robot = self.env.GetRobot('pr2')
self.ikmodel = inversekinematics.InverseKinematicsModel(
self.robot, iktype=IkParameterization.Type.Transform6D)
if not self.ikmodel.load():
self.ikmodel.autogenerate()
self.basemanip = BaseManipulation(self.robot)
self.taskmanip = None
self.updir = array((0, 0, 1))
#self.target = self.env.ReadKinBodyXMLFile('models/wg_database/18718.kinbody.xml')
self.target = self.env.ReadKinBodyXMLFile('data/mug2.kinbody.xml')
self.env.AddKinBody(self.target)
#self.target.SetTransform(array(mat([[1,0,0,.7],[0,1,0,0],[0,0,1,.673],[0,0,0,1]])))
self.target.SetTransform(
array(
mat([[1, 0, 0, .7], [0, 1, 0, 0], [0, 0, 1, .674],
[0, 0, 0, 1]])))
# init robot pose
self.leftarm = self.robot.GetManipulator('leftarm')
self.rightarm = self.robot.GetManipulator('rightarm')
v = self.robot.GetActiveDOFValues()
v[35] = 3.14 / 2 # l shoulder pan
v[56] = -3.14 / 2 # r shoulder pan
v[14] = .31 # torso
v[47] = .54 # l gripper
self.robot.SetActiveDOFValues(v)
self.gmodel = grasping.GraspingModel(robot=self.robot,
target=self.target)
if not self.gmodel.load():
self.gmodel.autogenerate(options=self.MyGrasperOptions())
self.taskmanip = TaskManipulation(
self.robot, graspername=self.gmodel.grasper.plannername)
self.display_handles = []
class MobileManipulationPlanning(metaclass.AutoReloader):
def __init__(self,robot,grmodel=None,switchpatterns=None,maxvelmult=None):
self.env=robot.GetEnv()
self.envreal = None
self.robot = robot
self.grmodel = grmodel
self.maxvelmult=maxvelmult
self.basemanip = BaseManipulation(self.robot,maxvelmult=maxvelmult)
self.taskmanip = TaskManipulation(self.robot,maxvelmult=maxvelmult)
self.switchpatterns=switchpatterns
if switchpatterns is not None:
self.taskmanip.SwitchModels(switchpatterns=switchpatterns)
self.updir = array((0,0,1))
def clone(self,envother):
clone = shallowcopy(self)
clone.env = envother
clone.envreal = self.env
clone.robot = clone.env.GetRobot(self.robot.GetName())
clone.grmodel = self.grmodel.clone(envother) if self.grmodel is not None else None
clone.basemanip = self.basemanip.clone(envother)
clone.taskmanip = self.taskmanip.clone(envother)
if clone.switchpatterns is not None:
clone.taskmanip.SwitchModels(switchpatterns=clone.switchpatterns)
return clone
def waitrobot(self):
"""busy wait for robot completion"""
while not self.robot.GetController().IsDone():
time.sleep(0.01)
time.sleep(0.1)
def graspObjectWithModels(self,allgmodels,usevisibilitycamera=None,neutraljointvalues=None):
viewmanip = None
if usevisibilitycamera:
# filter gmodel to be the same as the camera
gmodels = None
target = None
for testgmodel in allgmodels:
try:
target,viewmanip = self.viewTarget(usevisibilitycamera,testgmodel.target)
gmodels = [gmodel for gmodel in allgmodels if gmodel.target==target]
order = argsort([gmodel.manip.GetEndEffector()!=viewmanip.GetEndEffector() for gmodel in gmodels])
gmodels = [gmodels[i] for i in order]
break
except (RuntimeError,planning_error):
print('failed to view ',testgmodel.target.GetName())
else:
gmodels = allgmodels
if gmodels is None:
raise planning_error('graspObjectWithModels')
approachoffset = 0.03
stepsize = 0.001
graspiterators = [(gmodel,gmodel.validGraspIterator(checkik=True,randomgrasps=False,backupdist=approachoffset)) for gmodel in gmodels]
while(len(graspiterators)>0):
index=random.randint(len(graspiterators))
try:
with self.env:
grasp,graspindex=next(graspiterators[index][1])
except StopIteration:
graspiterators.pop(index)
continue
gmodel = graspiterators[index][0]
if viewmanip is not None and neutraljointvalues is not None and gmodel.manip.GetEndEffector()!=viewmanip.GetEndEffector():
try:
self.moveToNeutral(neutraljointvalues=neutraljointvalues,bounds=array(((-0.1,-0.1,-0.1),(0.1,0.1,0.1))))
neutraljointvalues=None
except planning_error as e:
print('failed to move to neutral values:',e)
print('selected %s grasp %d '%(gmodel.manip,graspindex))
gmodel.moveToPreshape(grasp)
time.sleep(1.5)
self.waitrobot()
with self.env:
self.robot.SetActiveManipulator(gmodel.manip)
Tgrasp=gmodel.getGlobalGraspTransform(grasp,collisionfree=True)
finalarmsolution = gmodel.manip.FindIKSolution(Tgrasp,filteroptions=IkFilterOptions.CheckEnvCollisions)
if finalarmsolution is None:
print('final grasp has no valid IK')
continue
armjoints=gmodel.manip.GetArmIndices()
Tfirstgrasp = array(Tgrasp)
Tfirstgrasp[0:3,3] -= approachoffset*gmodel.getGlobalApproachDir(grasp)
solutions = gmodel.manip.FindIKSolutions(Tfirstgrasp,filteroptions=IkFilterOptions.CheckEnvCollisions)
if solutions is None or len(solutions) == 0:
continue
# find the closest solution
weights = self.robot.GetDOFWeights()[armjoints]
dists = [numpy.max(abs(finalarmsolution-s)) for s in solutions]
index = argmin(dists)
usejacobian = False
if dists[index] < 15.0*approachoffset:
usejacobian = True
self.robot.SetActiveDOFs(armjoints)
print('moving to initial ',solutions[index])
try:
with TaskManipulation.SwitchState(self.taskmanip):
self.basemanip.MoveActiveJoints(goal=solutions[index],maxiter=6000,maxtries=2)
except planning_error as e:
usejacobian=False
else:
print('closest solution is too far',dists[index])
self.waitrobot()
if usejacobian:
try:
print('moving hand')
expectedsteps = floor(approachoffset/stepsize)
self.basemanip.MoveHandStraight(direction=gmodel.getGlobalApproachDir(grasp),ignorefirstcollision=False,stepsize=stepsize,minsteps=expectedsteps-2,maxsteps=expectedsteps+3,searchall=False)
except planning_error as e:
# try:
# self.basemanip.MoveHandStraight(direction=gmodel.getGlobalApproachDir(grasp),ignorefirstcollision=False,stepsize=stepsize,minsteps=expectedsteps-2,maxsteps=expectedsteps+1,searchall=True)
# except planning_error as e:
print('failed to move straight: ',e,' Using planning to move rest of the way.')
usejacobian = False
if not usejacobian:
print('moving to final ',solutions[index])
with self.env:
self.robot.SetActiveDOFs(armjoints)
self.basemanip.MoveActiveJoints(goal=finalarmsolution,maxiter=5000,maxtries=2)
self.waitrobot()
success = False
try:
self.closefingers(target=gmodel.target)
success = True
except planning_error as e:
print(e)
try:
self.basemanip.MoveHandStraight(direction=self.updir,stepsize=0.002,minsteps=1,maxsteps=100)
except:
print('failed to move up')
self.waitrobot()
if not success:
continue
return gmodel
raise planning_error('failed to grasp')
def graspObject(self,allgmodels,usevisibilitycamera=None,neutraljointvalues=None):
viewmanip = None
if usevisibilitycamera:
# filter gmodel to be the same as the camera
gmodels = None
target = None
for testgmodel in allgmodels:
try:
target,viewmanip = self.viewTarget(usevisibilitycamera,testgmodel.target)
gmodels = [gmodel for gmodel in allgmodels if gmodel.target==target]
order = argsort([gmodel.manip.GetEndEffector()!=viewmanip.GetEndEffector() for gmodel in gmodels])
gmodels = [gmodels[i] for i in order]
break
except (RuntimeError,planning_error):
print('failed to view ',testgmodel.target.GetName())
else:
gmodels = allgmodels
print('graspplanning grasp and place object')
for gmodel in gmodels:
try:
if viewmanip is not None and neutraljointvalues is not None and gmodel.manip.GetEndEffector()!=viewmanip.GetEndEffector():
try:
self.moveToNeutral(neutraljointvalues=neutraljointvalues,bounds=array(((-0.1,-0.1,-0.1),(0.1,0.1,0.1))))
neutraljointvalues=None
except planning_error as e:
print('failed to move to neutral values:',e)
self.robot.SetActiveManipulator(gmodel.manip)
approachoffset = 0.03
stepsize = 0.001
goals,graspindex,searchtime,trajdata = self.taskmanip.GraspPlanning(graspindices=gmodel.graspindices,grasps=gmodel.grasps,
target=gmodel.target,approachoffset=approachoffset,destposes=None,
seedgrasps = 3,seedik=3,maxiter=2000,randomgrasps=False)
except planning_error as e:
print('failed to grasp with %s'%(gmodel.manip),e)
continue
self.waitrobot()
usejacobian = False
armjoints = gmodel.manip.GetArmIndices()
self.robot.SetActiveDOFs(armjoints)
grasp=gmodel.grasps[graspindex]
with self.env:
# although values holds the final configuration, robot should approach from a distance of approachoffset
self.robot.SetActiveManipulator(gmodel.manip)
Tgrasp = gmodel.getGlobalGraspTransform(grasp,collisionfree=True)
finalarmsolution = gmodel.manip.FindIKSolution(Tgrasp,filteroptions=IkFilterOptions.CheckEnvCollisions)
if finalarmsolution is None:
print('final grasp has no valid IK')
continue
# find the closest solution
curjointvalues = self.robot.GetDOFValues(armjoints)
weights = self.robot.GetDOFWeights()[armjoints]
dist = numpy.max(abs(finalarmsolution-curjointvalues))
if dist < 15.0*approachoffset:
usejacobian = True
if usejacobian:
try:
print('moving hand')
expectedsteps = floor(approachoffset/stepsize)
self.basemanip.MoveHandStraight(direction=gmodel.getGlobalApproachDir(grasp),ignorefirstcollision=False,stepsize=stepsize,minsteps=expectedsteps-2,maxsteps=expectedsteps+1,searchall=False)
except planning_error as e:
try:
self.basemanip.MoveHandStraight(direction=gmodel.getGlobalApproachDir(grasp),ignorefirstcollision=False,stepsize=stepsize,minsteps=expectedsteps-2,maxsteps=expectedsteps+1,searchall=True)
except planning_error as e:
print('failed to move straight: ',e,' Using planning to move rest of the way.')
usejacobian = False
self.waitrobot()
if not usejacobian:
try:
with TaskManipulation.SwitchState(self.taskmanip):
print('moving active joints')
self.basemanip.MoveActiveJoints(goal=finalarmsolution)
except planning_error as e:
print(e)
continue
self.waitrobot()
self.closefingers(target=gmodel.target)
try:
self.basemanip.MoveHandStraight(direction=self.updir,jacobian=0.02,stepsize=0.002,minsteps=1,maxsteps=100)
except:
print('failed to move up')
self.waitrobot()
return gmodel
raise planning_error('failed to grasp target')
def moveToNeutral(self,neutraljointvalues,bounds=None,manipnames=None,ikcollisionbody=None):
"""moves the robot to a neutral position defined by several manipulators and neutraljointvalues. Can also specify a special collision body to constraint choosing the goals."""
if manipnames is None:
manipnames = ['leftarm','rightarm']
manips = [self.robot.GetManipulator(name) for name in manipnames]
ikmodels = []
for manip in manips:
self.robot.SetActiveManipulator(manip)
ikmodel = inversekinematics.InverseKinematicsModel(robot=self.robot,iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
ikmodels.append(ikmodel)
goaljointvalues = None
alltrajdata = None
with self.robot:
origjointvalues=self.robot.GetDOFValues()
self.robot.SetDOFValues(neutraljointvalues)
if ikcollisionbody is not None:
ikcollisionbody.Enable(True)
nocollision = not self.env.CheckCollision(self.robot) and not self.robot.CheckSelfCollision()
if ikcollisionbody is not None:
ikcollisionbody.Enable(False)
if nocollision:
goaljointvalues = neutraljointvalues
try:
alltrajdata = []
self.robot.SetDOFValues(origjointvalues)
for manip in manips:
self.robot.SetActiveDOFs(manip.GetArmIndices())
alltrajdata.append(self.basemanip.MoveActiveJoints(goal=goaljointvalues[manip.GetArmIndices()],execute=False,outputtraj=True))
self.robot.SetDOFValues(goaljointvalues[manip.GetArmIndices()],manip.GetArmIndices())
except planning_error:
alltrajdata = None
if alltrajdata is None:
self.robot.SetDOFValues(neutraljointvalues)
Tmanips = [dot(linalg.inv(manip.GetBase().GetTransform()),manip.GetEndEffectorTransform()) for manip in manips]
for niter in range(500):
self.robot.SetDOFValues(origjointvalues)
if bounds is None:
r = random.rand(3)*0.4-0.2
else:
r = bounds[0,:]+random.rand(3)*(bounds[1,:]-bounds[0,:])
success = True
startjointvalues=[]
for manip,Tmanip in izip(manips,Tmanips):
T = dot(manip.GetBase().GetTransform(),Tmanip)
if niter > 0:
T[0:3,3] += r
if niter > 200:
T[0:3,0:3] = dot(T[0:3,0:3],rotationMatrixFromAxisAngle(random.rand(3)*0.5))
s=None
try:
if ikcollisionbody is not None:
ikcollisionbody.Enable(True)
for link in manip.GetIndependentLinks():
link.Enable(False)
s = manip.FindIKSolution(T,filteroptions=IkFilterOptions.CheckEnvCollisions)
finally:
if ikcollisionbody is not None:
ikcollisionbody.Enable(False)
for link in manip.GetIndependentLinks():
link.Enable(True)
if s is None:
success = False
break
else:
if ikcollisionbody is not None:
# have to check self collision
with self.robot:
self.robot.SetDOFValues(s,manip.GetArmIndices())
if self.robot.CheckSelfCollision():
success = False
break
startjointvalues.append(self.robot.GetDOFValues())
self.robot.SetDOFValues(s,manip.GetArmIndices())
if not success:
continue
try:
print('attempting to plan...')
goaljointvalues = self.robot.GetDOFValues()
alltrajdata = []
for jointvalues,manip in izip(startjointvalues,manips):
self.robot.SetDOFValues(jointvalues)
self.robot.SetActiveDOFs(manip.GetArmIndices())
alltrajdata.append(self.basemanip.MoveActiveJoints(goal=goaljointvalues[manip.GetArmIndices()],execute=False,outputtraj=True))
break
except planning_error:
alltrajdata = None
if alltrajdata is None:
raise planning_error('failed to find collision free position')
for trajdata in alltrajdata:
self.basemanip.TrajFromData(trajdata)
self.waitrobot()
def searchrealenv(self,target=None,updateenv=False,waitfortarget=0):
"""sync with the real environment and find the target"""
if self.envreal is None:
return target,self.env
if updateenv:
print('updating entire environment not supported yet')
starttime = time.time()
while True:
with self.envreal:
# find target of the same name/type
try:
b = self.envreal.GetKinBody(target.GetName())
if b:
target.SetBodyTransformations(b.GetBodyTransformations())
return target,self.env
except:
pass
bodies = [b for b in self.envreal.GetBodies() if b.GetXMLFilename()==target.GetXMLFilename()]
if len(bodies) > 0:
# find the closest
dists = [sum((b.GetTransform()[0:3,3]-target.GetTransform()[0:3,3])**2) for b in bodies]
index = argmin(dists)
if dists[index] < 1:
print('distance to original: ',dists[index])
target.SetBodyTransformations(bodies[index].GetBodyTransformations())
return target, self.env
if time.time()-starttime > waitfortarget:
break
time.sleep(0.05)
raise planning_error('failed to recognize target')
def graspAndPlaceObjectMobileSearch(self,targetdests):
assert(len(targetdests)>0)
weight = 2.0
logllthresh = 0.5
origjointvalues = self.robot.GetDOFValues()
configsampler = self.grmodel.sampleValidPlacementIterator(weight=weight,logllthresh=logllthresh,randomgrasps=False,randomplacement=False,updateenv=False)
with self.env:
starttime=time.time()
while True:
pose,values,grasp,graspindex = next(configsampler)
gmodel = graspindex[0]
dests = [dests for target,dests in targetdests if target == gmodel.target]
if len(dests) == 0:
continue
print('found grasp in %fs'%(time.time()-starttime),gmodel.target.GetName(),graspindex[1])
# check all destinations
self.robot.SetActiveManipulator(gmodel.manip)
Tgrasp = gmodel.getGlobalGraspTransform(grasp,collisionfree=True)
Trelative = dot(linalg.inv(gmodel.target.GetTransform()),Tgrasp)
Ttarget = None
try:
gmodel.target.Enable(False)
with KinBodyStateSaver(self.robot):
self.robot.SetTransform(pose)
self.robot.SetDOFValues(values)
gmodel.setPreshape(grasp)
for T in dests[0]:
Tnewgrasp = dot(T,Trelative)
if gmodel.manip.FindIKSolution(Tnewgrasp,filteroptions=IkFilterOptions.CheckEnvCollisions) is not None:
Ttarget = T
break
if Ttarget is not None:
break
finally:
gmodel.target.Enable(True)
self.graspObjectMobile(pose,values,grasp,graspindex)
# if everything finished correctly, put object in destination
self.basemanip.MoveToHandPosition([Tnewgrasp],seedik=10,maxiter=5000)
self.waitrobot()
self.basemanip.ReleaseFingers(target=gmodel.target)
self.waitrobot()
# with self.env:
# self.robot.SetActiveDOFs(gmodel.manip.GetArmIndices())
# try:
# self.basemanip.MoveActiveJoints(goal=origjointvalues[gmodel.manip.GetArmIndices()],maxiter=3000)
# except:
# print('failed to move arm closer')
self.waitrobot()
def graspObjectMobileSearch(self,**kwargs):
weight = 2.0
logllthresh = 0.5
origjointvalues = self.robot.GetDOFValues()
configsampler = self.grmodel.sampleValidPlacementIterator(weight=weight,logllthresh=logllthresh,randomgrasps=False,randomplacement=False,updateenv=False)
with self.env:
starttime=time.time()
pose,values,grasp,graspindex = next(configsampler)
print('found in %fs'%(time.time()-starttime))
gmodel = self.graspObjectMobile(pose,values,grasp,graspindex,**kwargs)
# # move to original position
# with self.env:
# self.robot.SetActiveDOFs(gmodel.manip.GetArmIndices())
# try:
# self.basemanip.MoveActiveJoints(goal=origjointvalues[gmodel.manip.GetArmIndices()],maxiter=3000)
# except:
# print('failed to move arm closer')
# self.waitrobot()
return gmodel
def closefingers(self,manip=None,target=None):
"""close fingers and grab body"""
with self.env:
if manip:
self.robot.SetActiveManipulator(manip)
self.taskmanip.CloseFingers()
self.waitrobot()
with self.env:
expectedvalues = self.robot.GetDOFValues(self.robot.GetActiveManipulator().GetGripperIndices())
if target is not None:
with TaskManipulation.SwitchState(self.taskmanip): # grab the fat object!
with self.env:
self.robot.Grab(target)
if self.envreal is not None: # try to grab with the real environment
try:
with self.envreal:
robotreal = self.envreal.GetRobot(self.robot.GetName())
if robotreal is not None:
# always create a dummy body
targetreal = self.envreal.ReadKinBodyXMLFile(target.GetXMLFilename())
if target is not None:
targetreal.SetName(target.GetName()+str(100+random.randint(10000)))
self.envreal.AddKinBody(targetreal)
targetreal.SetBodyTransformations(target.GetBodyTransformations())
robotreal.SetActiveManipulator(self.robot.GetActiveManipulator().GetName())
robotreal.Grab(targetreal)
else:
print('failed to create real target with filename ',target.GetXMLFilename())
except openrave_exception as e:
print('closefingers:',e)
return expectedvalues
def releasefingers(self,manip=None):
"""open fingers and release body"""
with self.env:
if manip:
self.robot.SetActiveManipulator(manip)
# find the grabbing body
grabbed = [grabbedbody for grabbedbody in self.robot.GetGrabbed() if self.robot.GetActiveManipulator().IsGrabbing(grabbedbody)]
self.basemanip.ReleaseFingers(target=grabbed[0] if len(grabbed)>0 else None)
self.waitrobot()
if self.envreal is not None:
try:
with self.envreal:
robotreal = self.envreal.GetRobot(self.robot.GetName())
if robotreal is not None:
targets = robotreal.GetGrabbed()
robotreal.ReleaseAllGrabbed()
# for target in targets:
# self.envreal.Remove(target)
except openrave_exception as e:
print('releasefingers:',e)
def moveWithFreeArms(self,jointvalues,jointinds,maxtries=3):
with self.env:
self.robot.SetActiveDOFs(jointinds)
self.basemanip.MoveActiveJoints(goal=jointvalues,maxtries=maxtries,maxiter=5000)
self.waitrobot()
def graspObjectMobile(self,pose,values,grasp,graspindex,usevisibilitycamera=None):
approachoffset = 0.03
stepsize = 0.001
gmodel = graspindex[0]
print('found grasp',gmodel.manip.GetName(),gmodel.target.GetName(),graspindex[1])
self.robot.SetActiveManipulator(gmodel.manip)
self.moveBase(pose)
with self.env:
armjoints=gmodel.manip.GetArmIndices()
#finalarmsolution = values[armjoints]
dofindices = inversereachability.InverseReachabilityModel.getdofindices(gmodel.manip)
if len(dofindices) > 0:
print('moving for reachability: ',values[dofindices],dofindices)
self.moveWithFreeArms(values[dofindices],dofindices,maxtries=2)
self.robot.SetActiveManipulator(gmodel.manip)
if usevisibilitycamera:
newtarget,viewmanip = self.viewTarget(usevisibilitycamera,gmodel.target)
gmodel.moveToPreshape(grasp)
time.sleep(1.5)
self.waitrobot()
usejacobian = False
with self.env:
# although values holds the final configuration, robot should approach from a distance of approachoffset
Tgrasp = gmodel.getGlobalGraspTransform(grasp,collisionfree=True)
finalarmsolution = gmodel.manip.FindIKSolution(Tgrasp,filteroptions=IkFilterOptions.CheckEnvCollisions)
if finalarmsolution is None:
raise planning_error('final grasp has no valid IK')
Tfirstgrasp = array(Tgrasp)
Tfirstgrasp[0:3,3] -= approachoffset*gmodel.getGlobalApproachDir(grasp)
solutions = gmodel.manip.FindIKSolutions(Tfirstgrasp,filteroptions=IkFilterOptions.CheckEnvCollisions)
if solutions is None or len(solutions) == 0:
return self.graspObject([gmodel])
# find the closest solution
weights = self.robot.GetDOFWeights()[armjoints]
dists = [numpy.max(abs(finalarmsolution-s)) for s in solutions]
index = argmin(dists)
if dists[index] < 15.0*approachoffset:
usejacobian = True
with TaskManipulation.SwitchState(self.taskmanip):
self.robot.SetActiveDOFs(armjoints)
self.basemanip.MoveActiveJoints(goal=solutions[index],maxiter=5000,maxtries=2)
else:
print('closest solution is too far',dists[index],15.0*approachoffset)
self.waitrobot()
if usejacobian:
try:
print('moving hand')
expectedsteps = floor(approachoffset/stepsize)
self.basemanip.MoveHandStraight(direction=gmodel.getGlobalApproachDir(grasp),ignorefirstcollision=False,stepsize=stepsize,minsteps=expectedsteps-2,maxsteps=expectedsteps+1,searchall=False)
except:
print('failed to move straight, using planning to move rest of the way')
usejacobian = False
if not usejacobian:
with self.env:
self.robot.SetActiveDOFs(armjoints)
self.basemanip.MoveActiveJoints(goal=finalarmsolution,maxiter=5000,maxtries=2)
self.waitrobot()
success = False
try:
self.closefingers(target=gmodel.target)
success = True
except planning_error as e:
print(e)
try:
self.basemanip.MoveHandStraight(direction=self.updir,stepsize=0.002,minsteps=1,maxsteps=100)
except:
print('failed to move up')
self.waitrobot()
if not success:
raise planning_error('failed to close fingers')
return gmodel
def placeObjectMobileSearch(self,gmodel,dests,goalbounds,numtries=10,nummaxdests=10):
assert(len(dests)>0)
trajdata=None
pose = None
self.robot.SetActiveManipulator(gmodel.manip)
with self.robot:
dbounds = goalbounds[1,:]-goalbounds[0,:]
Trobot = self.robot.GetTransform()
Trelative = dot(linalg.inv(gmodel.target.GetTransform()),gmodel.manip.GetEndEffectorTransform())
for iter in range(numtries):
r = random.rand(3)
if iter > 0:
angle = 0.5*(goalbounds[0,0]+r[0]*dbounds[0])
pose = r_[cos(angle),0,0,sin(angle),goalbounds[0,1:3]+r[1:3]*dbounds[1:3],Trobot[2,3]]
else:
pose = poseFromMatrix(Trobot)
self.robot.SetTransform(pose)
if self.env.CheckCollision(self.robot):
continue
try:
I=random.permutation(range(len(dests)))[0:min(100,len(dests))]
trajdata = self.basemanip.MoveToHandPosition(matrices=[dot(dests[ind],Trelative) for ind in I],maxiter=4000,seedik=4,execute=False,outputtraj=True)
if trajdata is not None:
break
except planning_error:
pass
if pose is None or trajdata is None:
raise planning_error()
self.moveBase(pose)
self.basemanip.TrajFromData(trajdata)
self.waitrobot()
# move hand down
try:
self.basemanip.MoveHandStraight(direction=-self.updir,jacobian=0.02,stepsize=0.002,minsteps=0,maxsteps=100)
self.waitrobot()
self.basemanip.MoveHandStraight(direction=-self.updir,jacobian=0.02,ignorefirstcollision=True,stepsize=0.002,minsteps=0,maxsteps=5)
self.waitrobot()
except planning_error as e:
print('failed to move hand down')
traceback.print_exc(e)
self.releasefingers()
def placeObject(self,target,dests):
assert(len(dests)>0)
with self.robot:
Trelative = dot(linalg.inv(target.GetTransform()),self.robot.GetActiveManipulator().GetEndEffectorTransform())
I=random.permutation(range(len(dests)))
num = int(len(I)/50)
success = False
for i in range(num):
try:
self.basemanip.MoveToHandPosition(matrices=[dot(dests[ind],Trelative) for ind in I[i::num]],maxiter=4000,seedik=4)
success = True
break
except planning_error:
pass
if not success:
raise planning_error('failed to place object in one of dests')
self.waitrobot()
# move hand down
try:
print(self.robot.GetActiveManipulator())
print(self.robot.GetDOFValues())
self.basemanip.MoveHandStraight(direction=-self.updir,jacobian=0.02,stepsize=0.002,minsteps=0,maxsteps=100)
self.waitrobot()
self.basemanip.MoveHandStraight(direction=-self.updir,jacobian=0.02,ignorefirstcollision=True,stepsize=0.002,minsteps=0,maxsteps=5)
self.waitrobot()
except planning_error as e:
print('failed to move hand down')
traceback.print_exc(e)
self.releasefingers()
def moveBase(self,pose):
with self.robot:
self.robot.SetTransform(pose)
if self.env.CheckCollision(self.robot):
raise planning_error('goal position in self collision')
with self.robot: # find the boundaries of the environment
envmin = []
envmax = []
for b in self.env.GetBodies():
ab = b.ComputeAABB()
envmin.append(ab.pos()-ab.extents())
envmax.append(ab.pos()+ab.extents())
abrobot = self.robot.ComputeAABB()
envmin = numpy.min(array(envmin),0)+abrobot.extents()
envmax = numpy.max(array(envmax),0)-abrobot.extents()
bounds = array(((envmin[0],envmin[1],-pi),(envmax[0],envmax[1],pi)))
self.robot.SetAffineTranslationLimits(envmin,envmax)
self.robot.SetAffineTranslationMaxVels([0.5,0.5,0.5])
self.robot.SetAffineRotationAxisMaxVels(ones(4))
self.robot.SetActiveDOFs([],Robot.DOFAffine.X|Robot.DOFAffine.Y|Robot.DOFAffine.RotationAxis,[0,0,1])
goal2d = [pose[4],pose[5],2.0*numpy.arctan2(pose[3],pose[0])]
print('planning to: ',goal2d)
center = r_[goal2d[0:2],0.2]
xaxis = 0.5*array((cos(goal2d[2]),sin(goal2d[2]),0))
yaxis = 0.25*array((-sin(goal2d[2]),cos(goal2d[2]),0))
self.hgoal = self.env.drawlinelist(transpose(c_[center-xaxis,center+xaxis,center-yaxis,center+yaxis]),linewidth=5.0,colors=array((0,1,0)))
if self.basemanip.MoveActiveJoints(goal=goal2d,maxiter=3000,steplength=0.1) is None:
raise planning_error('failed to plan goal position')
self.waitrobot()
def viewTarget(self,usevisibilitycamera,target):
print('attempting visibility to ',target.GetName(),' planning with ',usevisibilitycamera['sensorname'])
vmodel = visibilitymodel.VisibilityModel(robot=self.robot,target=target,sensorname=usevisibilitycamera['sensorname'],maxvelmult=self.maxvelmult)
if not vmodel.load():
raise planning_error('failed to load visibility model')
#pts = array([dot(vmodel.target.GetTransform(),matrixFromPose(pose))[0:3,3] for pose in vmodel.visibilitytransforms])
#h=vmodel.env.plot3(pts,10,colors=array([1,0.7,0,0.05]))
reachabletransforms = vmodel.pruneTransformations(thresh=0.04,numminneighs=40,maxdist=usevisibilitycamera.get('maxdist',0.25))
vmodel.SetCameraTransforms(reachabletransforms)
for iter in range(20):
try:
vmodel.visualprob.MoveToObserveTarget(sampleprob=0.001,maxiter=4000)
except RuntimeError as e:
print(e)
raise planning_error('cannot find target')
except planning_error as e:
print(e)
continue
#s=vmodel.visualprob.SampleVisibilityGoal(target)
self.waitrobot()
if usevisibilitycamera.get('dosync',False):
if usevisibilitycamera.get('ask',False):
cmd=input('press n key to capture new environment: ')
if cmd == 'n':
continue
with self.env:
time.sleep(usevisibilitycamera.get('syncdelay',1.0))
try:
newtarget,newenv = self.searchrealenv(target,waitfortarget=4.0)
if newtarget is not None:
break
except planning_error as e:
print(e)
continue
else:
newtarget = target
break
if newtarget is None:
raise planning_error('cannot find target')
if usevisibilitycamera.get('storeimage',False):
usevisibilitycamera['image']= vmodel.getCameraImage()
print(repr(self.robot.GetDOFValues()))
print(repr(self.robot.GetTransform()))
return target,vmodel.manip
class MobileManipulationPlanning(metaclass.AutoReloader):
def __init__(self,robot,grmodel=None,switchpatterns=None,maxvelmult=None):
self.env=robot.GetEnv()
self.envreal = None
self.robot = robot
self.grmodel = grmodel
self.maxvelmult=maxvelmult
self.basemanip = BaseManipulation(self.robot,maxvelmult=maxvelmult)
self.taskmanip = TaskManipulation(self.robot,maxvelmult=maxvelmult)
self.switchpatterns=switchpatterns
if switchpatterns is not None:
self.taskmanip.SwitchModels(switchpatterns=switchpatterns)
self.updir = array((0,0,1))
def clone(self,envother):
clone = shallowcopy(self)
clone.env = envother
clone.envreal = self.env
clone.robot = clone.env.GetRobot(self.robot.GetName())
clone.grmodel = self.grmodel.clone(envother) if self.grmodel is not None else None
clone.basemanip = self.basemanip.clone(envother)
clone.taskmanip = self.taskmanip.clone(envother)
if clone.switchpatterns is not None:
clone.taskmanip.SwitchModels(switchpatterns=clone.switchpatterns)
return clone
def waitrobot(self):
"""busy wait for robot completion"""
while not self.robot.GetController().IsDone():
time.sleep(0.01)
time.sleep(0.1)
def graspObjectWithModels(self,allgmodels,usevisibilitycamera=None,neutraljointvalues=None):
viewmanip = None
if usevisibilitycamera:
# filter gmodel to be the same as the camera
gmodels = None
target = None
for testgmodel in allgmodels:
try:
target,viewmanip = self.viewTarget(usevisibilitycamera,testgmodel.target)
gmodels = [gmodel for gmodel in allgmodels if gmodel.target==target]
order = argsort([gmodel.manip.GetEndEffector()!=viewmanip.GetEndEffector() for gmodel in gmodels])
gmodels = [gmodels[i] for i in order]
break
except (RuntimeError,planning_error):
print 'failed to view ',testgmodel.target.GetName()
else:
gmodels = allgmodels
if gmodels is None:
raise planning_error('graspObjectWithModels')
approachoffset = 0.03
stepsize = 0.001
graspiterators = [(gmodel,gmodel.validGraspIterator(checkik=True,randomgrasps=False,backupdist=approachoffset)) for gmodel in gmodels]
while(len(graspiterators)>0):
index=random.randint(len(graspiterators))
try:
with self.env:
grasp,graspindex=graspiterators[index][1].next()
except StopIteration:
graspiterators.pop(index)
continue
gmodel = graspiterators[index][0]
if viewmanip is not None and neutraljointvalues is not None and gmodel.manip.GetEndEffector()!=viewmanip.GetEndEffector():
try:
self.moveToNeutral(neutraljointvalues=neutraljointvalues,bounds=array(((-0.1,-0.1,-0.1),(0.1,0.1,0.1))))
neutraljointvalues=None
except planning_error,e:
print 'failed to move to neutral values:',e
print 'selected %s grasp %d '%(gmodel.manip,graspindex)
gmodel.moveToPreshape(grasp)
time.sleep(1.5)
self.waitrobot()
with self.env:
self.robot.SetActiveManipulator(gmodel.manip)
Tgrasp=gmodel.getGlobalGraspTransform(grasp,collisionfree=True)
finalarmsolution = gmodel.manip.FindIKSolution(Tgrasp,filteroptions=IkFilterOptions.CheckEnvCollisions)
if finalarmsolution is None:
print 'final grasp has no valid IK'
continue
armjoints=gmodel.manip.GetArmIndices()
Tfirstgrasp = array(Tgrasp)
Tfirstgrasp[0:3,3] -= approachoffset*gmodel.getGlobalApproachDir(grasp)
solutions = gmodel.manip.FindIKSolutions(Tfirstgrasp,filteroptions=IkFilterOptions.CheckEnvCollisions)
if solutions is None or len(solutions) == 0:
continue
# find the closest solution
weights = self.robot.GetDOFWeights()[armjoints]
dists = [numpy.max(abs(finalarmsolution-s)) for s in solutions]
index = argmin(dists)
usejacobian = False
if dists[index] < 15.0*approachoffset:
usejacobian = True
self.robot.SetActiveDOFs(armjoints)
print 'moving to initial ',solutions[index]
try:
with TaskManipulation.SwitchState(self.taskmanip):
self.basemanip.MoveActiveJoints(goal=solutions[index],maxiter=6000,maxtries=2)
except planning_error,e:
usejacobian=False
else: