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 test_reachability():
import kinematicreachability
env = Environment()
robot = env.ReadRobotXMLFile('robots/barrettsegway.robot.xml')
env.AddRobot(robot)
self = kinematicreachability.ReachabilityModel(robot=robot)
self.autogenerate()
def pruneTransformations(self,
thresh=0.04,
numminneighs=10,
maxdist=None,
translationonly=True):
if self.rmodel is None:
self.rmodel = kinematicreachability.ReachabilityModel(
robot=self.robot)
if not self.rmodel.load():
# do not autogenerate since that would force this model to depend on the reachability
self.rmodel = None
return array(visibilitytransforms)
kdtree = self.rmodel.ComputeNN(translationonly)
if maxdist is not None:
visibilitytransforms = self.visibilitytransforms[
invertPoses(self.visibilitytransforms)[:, 6] < maxdist]
else:
visibilitytransforms = self.visibilitytransforms
newtrans = poseMultArrayT(
poseFromMatrix(
dot(linalg.inv(self.manip.GetBase().GetTransform()),
self.target.GetTransform())), visibilitytransforms)
if translationonly:
transdensity = kdtree.kFRSearchArray(newtrans[:, 4:7], thresh**2,
0, thresh * 0.01)[2]
I = flatnonzero(transdensity > numminneighs)
return visibilitytransforms[I[argsort(-transdensity[I])]]
raise ValueError('not supported')
