def test_inversereachabilitygen():
import inversereachability
env = Environment()
robot = env.ReadRobotXMLFile('robots/barrettsegway.robot.xml')
env.AddRobot(robot)
self = inversereachability.InverseReachabilityModel(robot=robot)
heightthresh=0.05
quatthresh=0.15
self.generate(heightthresh=heightthresh,quatthresh=quatthresh)
def draw_inversereachability():
import inversereachability
env = openravepy.Environment()
robot = env.ReadRobotXMLFile('robots/barrettsegway.robot.xml')
env.AddRobot(robot)
self = inversereachability.InverseReachabilityModel(robot=robot)
self.load()
self.env.SetViewer('qtcoin')
self.showEquivalenceClass(self.equivalenceclasses[0])
def test_inversereachabilitytest():
import inversereachability
from scipy.optimize import leastsq
import bisect
env = openravepy.Environment()
robot = env.ReadRobotXMLFile('robots/barrettsegway.robot.xml')
env.AddRobot(robot)
self = inversereachability.InverseReachabilityModel(robot=robot)
self.load()
self.env.StopSimulation()
self.robot.SetTransform(eye(4))
self.testSampling(heights=arange(-1,1,0.1),logllthresh=2.3)
W = array([sum(e[2][:,-1]) for e in self.equivalenceclasses])
W /= max(W)
dirs = array([m[4]*linalg.inv(rotationMatrixFromQuat(m[0:4]))[:,2] for m in self.equivalencemeans])
h = self.env.plot3 (points=dirs,pointsize=5.0,colors=c_[1-W,1-W,1-W,W])
def test_inversereachabilityrun():
import inversereachability
env = Environment()
env.SetViewer('qtcoin')
env.Reset()
env.Load('data/lab1.env.xml')
robot = env.GetRobots()[0]
self = inversereachability.InverseReachabilityModel(robot=robot)
self.load()
gp = examples.graspplanning.GraspPlanning(robot=robot,randomize=False)
gm = gp.graspables[0][0]
dests = gp.graspables[0][1]
validgrasps,validindices = gm.computeValidGrasps()
Tgrasp = gm.getGlobalGraspTransform(validgrasps[0])
densityfn,samplerfn,bounds = self.computeBaseDistribution(Tgrasp,2000)
h = self.showBaseDistribution(densityfn,bounds,zoffset=1.0,thresh=1.0)
densityfn2,samplerfn2,bounds2 = self.computeAggregateBaseDistribution([Tgrasp],2000)
h2 = self.showBaseDistribution(densityfn2,bounds2,zoffset=3.0,thresh=1.0)
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()