def test_pr2():
from openravepy.examples import inversekinematics, grasping
env=Environment()
robot=env.ReadRobotXMLFile('robots/pr2-beta-sim.robot.xml')
env.AddRobot(robot)
# kinematics
# python inversekinematics.py --robot=robots/pr2-beta-sim.robot.xml --manipname=rightarm --freejoint=r_shoulder_pan_joint --numiktests=10
# python inversekinematics.py --robot=robots/pr2-beta-sim.robot.xml --manipname=rightarm_torso --freejoint=r_shoulder_pan_joint --freejoint=torso_lift_joint --numiktests=10
manipnames = ['leftarm','rightarm','leftarm_torso','rightarm_torso']
for manipname in manipnames:
manip=robot.SetActiveManipulator(manipname)
ikmodel=inversekinematics.InverseKinematicsModel(robot=robot,iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
rmodel = kinematicreachability.ReachabilityModel(robot)
if not rmodel.load():
rmodel.autogenerate()
# grasping
# python grasping.py --robot=robots/pr2-beta-sim.robot.xml --target=data/box_frootloops.kinbody.xml --boxdelta=0.01 --standoff=0 --standoff=0.02 --standoff=0.05 --normalanglerange=1 --roll=0 --roll=1.5707963 --roll=3.141592 --roll=4.7123889 --graspingnoise=0.01
with env:
target=env.ReadKinBodyXMLFile('data/box_frootloops.kinbody.xml')
env.AddKinBody(target)
for manipname in ['leftarm','rightarm']:
robot.SetActiveManipulator(manipname)
gmodel = grasping.GraspingModel(robot,target)
if not gmodel.load():
with gmodel.target:
gmodel.target.Enable(False)
final,traj = gmodel.basemanip.ReleaseFingers(execute=False,outputfinal=True)
gmodel.generate(preshapes = array([final]),rolls=arange(0,2*pi,pi/2),graspingnoise=0.01,standoffs=[0,0.02,0.05],approachrays=gmodel.computeBoxApproachRays(0.01,normalanglerange=1,directiondelta=0.1))
def preprocess(self):
with self.env:
manipname = self.visualprob.SetCameraAndTarget(
sensorname=self.sensorname,
sensorrobot=self.sensorrobot,
manipname=self.manipname,
target=self.target)
assert (self.manipname is None or self.manipname == manipname)
self.manip = self.robot.SetActiveManipulator(manipname)
self.attachedsensor = [
s for s in self.sensorrobot.GetAttachedSensors()
if s.GetName() == self.sensorname
][0]
self.ikmodel = inversekinematics.InverseKinematicsModel(
robot=self.robot, iktype=IkParameterization.Type.Transform6D)
if not self.ikmodel.load():
self.ikmodel.autogenerate()
if self.visibilitytransforms is not None:
self.visualprob.SetCameraTransforms(
transforms=self.visibilitytransforms)
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()