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_autograsping():
import grasping
env = Environment()
env.SetDebugLevel(DebugLevel.Debug)
env.SetViewer('qtcoin')
env.Load('data/lab1.env.xml')
robot = env.GetRobots()[0]
target = env.GetKinBody('mug1')
self = grasping.GraspingModel(robot=robot,target=target)
self.autogenerate()
def test_visibility():
import visibilitymodel
env = Environment()
env.Reset()
robot = env.ReadRobotXMLFile('robots/hrp2jsk08real.robot.xml')
env.AddRobot(robot)
robot.SetActiveManipulator('rightarm_chest')
target = env.ReadKinBodyXMLFile('scenes/cereal_frootloops.kinbody.xml')
env.AddKinBody(target)
robot.SetActiveManipulator('rightarm_chest')
self = visibilitymodel.VisibilityModel(robot=robot,target=target,sensorname='wristcam')
if not self.load():
self.autogenerate()
env.SetViewer('qtcoin')
self.showtransforms()
import kinematicreachability, grasping, visibilitymodel
pose = array([-0.88932403, 0. , 0. , -0.45727755, 8.26839721, 3.14201928, 0.66500002])
grasp = array([ 1.65648009e-06, -2.83057716e-06, 1.00000000e+00,
9.23879445e-01, 3.82683724e-01, -4.47596904e-07,
-3.82683724e-01, 9.23879445e-01, 3.24873599e-06,
-4.32802886e-02, 8.53489910e-04, 7.89998472e-02,
0.00000000e+00, 4.71238898e+00, 9.23879477e-01,
3.82683432e-01, -5.50675114e-08, 1.65648009e-06,
-2.83057716e-06, 1.00000000e+00, 9.23879445e-01,
3.82683724e-01, -4.47596904e-07, -3.82683724e-01,
9.23879445e-01, 3.24873599e-06, -4.23564091e-02,
1.23617332e-03, 7.89998472e-02, 6.41213000e-03,
-2.11999994e-02, 9.99999978e-03, 7.90000036e-02,
2.35619450e+00])
robot.SetTransform(pose)
robot.SetActiveManipulator('rightarm_chest')
# rmodel = kinematicreachability.ReachabilityModel(robot=robot)
# rmodel.load()
gmodel = grasping.GraspingModel(robot,target=env.GetKinBody('cereal0'))
gmodel.moveToPreshape(grasp)
gmodel.robot.GetController().Reset(0)
vmodel = visibilitymodel.VisibilityModel(robot=robot,target=gmodel.target,sensorname='wristcam')
vmodel.load()
self = vmodel
self.SetCameraTransforms(self.pruneTransformations(thresh=0.04))
validjoints=self.computeValidTransform()
self.robot.SetJointValues(validjoints[0][0],self.manip.GetArmJoints())
s=vmodel.visualprob.SampleVisibilityGoal(target=gmodel.target)
pts = array([dot(self.target.GetTransform(),matrixFromPose(pose))[0:3,3] for pose in self.visibilitytransforms])
h=self.env.plot3(pts,5,colors=array([0.5,0.5,1,0.03]))
def test_grasping():
import grasping
env = Environment()
robot = env.ReadRobotXMLFile('robots/barrettsegway.robot.xml')
env.AddRobot(robot)
T = eye(4)
T[0,3] = 1
robot.SetTransform(T)
target = env.ReadKinBodyXMLFile('data/mug1.kinbody.xml')
target.SetTransform(T)
env.AddKinBody(target)
env.SetViewer('qtcoin')
self = grasping.GraspingModel(robot=robot,target=target)
self.init(friction=0.4,avoidlinks=[])
preshapes = array(((0.5,0.5,0.5,pi/3),(0.5,0.5,0.5,0),(0,0,0,pi/2)))
rolls = arange(0,2*pi,pi/2)
standoffs = array([0,0.025])
approachrays = self.computeBoxApproachRays(stepsize=0.02)
graspingnoise=None
self.generate(preshapes=preshapes, rolls=rolls, standoffs=standoffs, approachrays=approachrays,graspingnoise=None,addSphereNorms=False)