def test_mobilemanipulation():
import mobilemanipulation
env = Environment()
env.SetViewer('qtcoin')
env.Reset()
env.Load('data/lab1.env.xml')
robot = env.GetRobots()[0]
self = mobilemanipulation.MobileManipulationPlanning(robot)
gmodel=self.graspables[0][0]
dests=self.graspables[0][1]
def test_navigation():
import inversereachability,mobilemanipulation,graspplanning,visibilitymodel
env = Environment()
env.SetViewer('qtcoin')
env.Reset()
env.Load('scenes/r602kitchen1.env.xml')
robot = env.GetRobots()[0]
robot.GetController().Reset(0)
Tstart = array([[ 1. , 0. , 0. , 6.26000023-7.37],
[ 0. , 1. , 0. , 2.66899991-3.2],
[ 0. , 0. , 1. , 0.66500002],
[ 0. , 0. , 0. , 1. ]])
robot.SetTransform(Tstart)
robot.SetJointValues(array([ 0. , 0. , 0.84761411, 0. , 0. ,
-2.20907021, 0. , 0. , 0. , 0.97831494,
0. , 0. , -2.23727012, 0. , 0. ,
0. , 0. , 0. , -0.17453291, 0.17453295], dtype=float32))
self = mobilemanipulation.MobileManipulationPlanning(robot)
goal2d = array([0.29134295742674898, -0.26705494655604034, -3.1834347453894472])
#goal2d = array([6.2547940332687197-7.37, 2.2240884123771689-3.2, -6.0887479146975627])
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])
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)))
env.SetDebugLevel(DebugLevel.Debug)
starttime = time.time()
self.basemanip.MoveActiveJoints(goal=goal2d,maxiter=3000,steplength=0.05)
print(time.time()-starttime)
def test_hrp2():
python convexdecomposition.py --volumeSplitThresholdPercent=5 --mergeThresholdPercent=10 --padding=0.005
rosrun openrave_database kinematicreachability_ros.py --manipname=leftarm --xyzdelta=0.04 --launchservice='8*localhost'
python kinematicreachability.py --manipname=rightarm --xyzdelta=0.02
python kinematicreachability.py --manipname=leftarm --xyzdelta=0.02
python kinematicreachability.py --manipname=rightarm_chest --xyzdelta=0.02
python inversereachability.py --manipname=rightarm --heightthresh=0.02 --quatthresh=0.2
python inversereachability.py --manipname=leftarm --heightthresh=0.02 --quatthresh=0.2
python inversereachability.py --manipname=rightarm_chest --heightthresh=0.02 --quatthresh=0.2
python inversereachability.py --manipname=leftarm_chest --heightthresh=0.02 --quatthresh=0.2
python inversereachability.py --manipname=leftarm_chest --heightthresh=0.02 --quatthresh=0.2 --id=0 --jointvalues='0'
python inversereachability.py --manipname=leftarm_chest --heightthresh=0.02 --quatthresh=0.2 --id=43 --jointvalues='0.43'
python grasping.py --robot=robots/hrp2jsk.robot.xml --manipname=rightarm --target=scenes/cereal_frootloops.kinbody.xml --standoff=0 --boxdelta=0.01 --normalanglerange=1 --avoidlink=RWristCam
python grasping.py --robot=robots/hrp2jsk.robot.xml --manipname=leftarm --target=scenes/cereal_frootloops.kinbody.xml --standoff=0 --boxdelta=0.01 --normalanglerange=1 --graspingnoise=0.01 --noviewer
rosrun openrave_database grasping_ros.py --robot=robots/hrp2jsk.robot.xml --manipname=leftarm_chest --target=scenes/cereal_frootloops.kinbody.xml --standoff=0 --boxdelta=0.01 --normalanglerange=1 --graspingnoise=0.01 --launchservice='8*localhost'
rosrun openrave_database grasping_ros.py --robot=robots/hrp2jsk.robot.xml --manipname=leftarm_chest2 --target=scenes/jskcup0.kinbody.xml --standoff=0 --boxdelta=0.01 --normalanglerange=1 --graspingnoise=0.01 --launchservice='8*localhost'
import inversereachability
env = Environment()
robot = env.ReadRobotXMLFile('robots/hrp2jsk.robot.xml')
env.AddRobot(robot)
robot.SetActiveManipulator('leftarm')
self = inversereachability.InverseReachabilityModel(robot=robot)
heightthresh=0.02
quatthresh=0.1
self.generate(heightthresh=heightthresh,quatthresh=quatthresh)
hand = env.ReadRobotXMLFile('robots/hrp2rhandjsk.robot.xml')
env.AddRobot(hand)
hand.SetTransform(Tgrasp)
# test head movement
import inversekinematics
env = Environment()
robot = env.ReadRobotXMLFile('robots/hrp2jsk08.robot.xml')
env.AddRobot(robot)
robot.SetActiveManipulator('head')
manip = robot.GetActiveManipulator()
ikmodel = inversekinematics.InverseKinematicsModel(robot,IkParameterization.Type.Direction3D)
if not ikmodel.load():
ikmodel.generate()
import inversereachability,mobilemanipulation,graspplanning,visibilitymodel
env = Environment()
env.SetViewer('qtcoin')
env.Reset()
env.Load('scenes/r602kitchen1.env.xml')
robot = env.GetRobots()[0]
origjointvalues = robot.GetJointValues()
# define all the manipulators to use
manips = [robot.GetManipulators('rightarm_chest')[0], robot.GetManipulators('rightarm_chest2')[0]]#,robot.GetManipulators('leftarm_chest')[0], robot.GetManipulators('leftarm_chest2')[0]]
irmodels = []
with robot:
for manip in manips:
robot.SetActiveManipulator(manip)
dofindices = inversereachability.InverseReachabilityModel.getdofindices(manip)
for id,value in [('0',[0]),('43',[0.43]),('n43',[-0.43])]:
robot.SetJointValues(value,dofindices)
irmodel = inversereachability.InverseReachabilityModel(robot=robot,id=id)
if irmodel.load():
irmodels.append(irmodel)
else:
print('failed to load irmodel',manip.GetName(),id)
irgmodels = []
targets = []
for manip in manips:
robot.SetActiveManipulator(manip)
planning = graspplanning.GraspPlanning(robot,nodestinations=True)
for gmodel,dests in planning.graspables:
if True:#gmodel.target.GetName() == 'cereal0' or gmodel.target.GetName() == 'cereal1':
for irmodel in irmodels:
if irmodel.manip == gmodel.manip:
irgmodels.append([irmodel,gmodel])
if not gmodel.target in targets:
targets.append(gmodel.target)
grmodel = mobilemanipulation.GraspReachability(robot=robot,irgmodels=irgmodels)
self = mobilemanipulation.MobileManipulationPlanning(robot,grmodel=grmodel)
usevisibilitycamera = 'wristcam'
gmodel = self.graspObjectMobileSearch(usevisibilitycamera=usevisibilitycamera)
table = env.GetKinBody('table')
if table is not None:
graspables = None
Trolls = [matrixFromAxisAngle(array((0,0,1)),roll) for roll in arange(0,2*pi,pi/4)]
alldests = graspplanning.GraspPlanning.setRandomDestinations(targets,table,transdelta=0.05,Trolls=Trolls,randomize=False)
targetdests=zip(targets,alldests)
self.graspAndPlaceObjectMobileSearch(targetdests=targetdests)
#h = gr.showBaseDistribution(thresh=1.0,logllthresh=logllthresh)
#grmodel.testSampling(weight=1.5,logllthresh=0.5,randomgrasps=True,randomplacement=False,updateenv=False)
validgrasps,validindices = gr.gmodel.computeValidGrasps(checkik=False,backupdist=0.01)
grmodel.gmodel.showgrasp(validgrasps[0],collisionfree=True)
densityfn,samplerfn,bounds,validgrasps = gr.computeGraspDistribution(logllthresh=logllthresh)
goals,numfailures=gr.sampleGoals(lambda goals: samplerfn(goals,weight=1.0),updateenv=True)
grasp,pose,q = goals[0]
robot.SetTransform(pose)
robot.SetJointValues(q)
basemanip.CloseFingers()
grasp = gr.gmodel.grasps[283]
Tgrasp = gr.gmodel.getGlobalGraspTransform(grasp,collisionfree=True)
equivalenceclass,logll = gr.irmodel.getEquivalenceClass(Tgrasp)
densityfn,samplerfn,bounds = gr.irmodel.computeBaseDistribution(Tgrasp,logllthresh=logllthresh)
h = gr.irmodel.showBaseDistribution(densityfn,bounds,zoffset=gr.target.GetTransform()[2,3],thresh=1.0)
env = Environment()
robot = env.ReadRobotXMLFile('robots/hrp2jsk08.robot.xml')
env.AddRobot(robot)
body = env.ReadKinBodyXMLFile('scenes/cereal_frootloops.kinbody.xml')
env.AddKinBody(body)
T = eye(4)
T[0:3,3] = [0.466,-0.157,0.544]
body.SetTransform(T)
robot.SetActiveManipulator('rightarm_chest')
robot.Grab(body)
robot.SetJointValues([-1.4,1.35239005,1.036349],[5,7,8])
robot.CheckSelfCollision()