def manip_from_pose_grasp(pose, grasp):
# This is the robot wrist
manip_trans = pose.value.compose(grasp.grasp_trans)
# Ad-hoc backoff strategy
if abs(manip_trans.matrix[2,0]) < 0.1: # horizontal
offset = hu.Pose(-glob.approachBackoff,0.,glob.approachPerpBackoff,0.)
else: # vertical
offset = hu.Pose(-glob.approachBackoff,0.,0.,0.)
manip_trans_approach = manip_trans.compose(offset)
# The grasp and the approach
return manip_trans, manip_trans_approach
def test0():
var = tinyVar # bigVar | tinyVar
exp = Experiment(
{'tableIkea1': (hu.Pose(1.3, 0.0, 0.0, math.pi / 2.0), var)},
{'objA': (hu.Pose(1.1, 0.0, ikZ, 0.0), var)},
['tableIkea1Top', 'tableIkea1Left'],
easy=False) # easy replaces the variance
#goal = emptyHand(hand)
#goal = holding('objA', hand=hand, graspType=grasp_type)
goal = inRegion(['objA'], 'tableIkea1Left')
return exp, goal
def testBusy(hardSwap=True, **args):
glob.rebindPenalty = 40
# Put this back to make the problem harder
#back = hu.Pose(1.1, 0.0, tZ, 0.0)
back = hu.Pose(1.45, 0.0, tZ, 0.0)
#parking1 = hu.Pose(1.15, 0.3, tZ, 0.0)
#parking2 = hu.Pose(1.15, -0.3, tZ, 0.0)
exp = Experiment(
{
'table1': (table1Pose, smallVar),
'table2': (table2Pose, smallVar)
}, {
'objA': (back, medVar),
'objB': (hu.Pose(1.15, -0.4, tZ, 0.0), medVar),
'objC': (hu.Pose(0.65, -1.2, tZ, 0.0), medVar),
'objD': (hu.Pose(1.15, -0.2, tZ, 0.0), medVar),
'objE': (hu.Pose(1.15, 0.0, tZ, 0.0), medVar),
'objF': (hu.Pose(1.15, 0.2, tZ, 0.0), medVar),
'objG': (hu.Pose(1.15, 0.4, tZ, 0.0), medVar)
}, ['table1Top', 'table2Top', 'table1MidFront', 'table1MidRear'],
easy=args.get('easy', False))
goal = inRegion(['objA', 'objB'],
['table1MidFront', 'table1MidRear']) # A on other table
goal1 = inRegion('objA', 'table2Top') # A and B on other table
goal2 = inRegion(['objA', 'objB'], 'table2Top') # B in back
goal3 = inRegion('objB', 'table1MidRear')
actualGoal = goal if hardSwap else goal3
return exp, actualGoal
def box_body(env, length, width, height, name='box', color='black'):
def trans(graspIndex):
# We're using the origin of object, so don't need to specify restFace
wrist = objectGraspFrameAux(gd, graspIndex, graspMu,
None, None, Ident,
robot, hand, origin=True)
return wrist
shape, _ = glob.constructor[shape_name](dx=length/2., dy=width/2., dz=height, name=name, color=color)
if length <= 0.7 and width <= 0.7: # graspable
gd = env.world.getObjData(shape_name, 'graspDesc', [])
hand = env.arm
robot = env.robot
graspMu = hu.Pose(0.0, -0.025, 0.0, 0.0) # Note !! constant
Ident = hu.Pose(0,0,0,0)
env.grasps[name] = [Grasp(name, i, trans(i)) for i,_ in enumerate(gd)]
else:
env.grasps[name] = []
return Body(env, shape)
def make_place(hand,
obj,
rest_face,
mu_pose,
grasp_conf,
approach_conf,
var=tinyVar):
# rest_face is just for updating discrete distribution over resting face
#face_frames = world.getFaceFrames(obj)
face_frames = None # Not actually used in the operator
pb = ObjPlaceB(obj, face_frames, rest_face, PoseD(hu.Pose(*mu_pose), var))
return ('Place', (approach_conf, grasp_conf, hand, pb))
def openrave_test(
env): # NOTE - cannot do Leslie and Tomas's viewers with openrave
test = testBusy # test0 | testBusy
exp, _ = test()
test = PlanTest(exp, noWindow=True)
test.load()
conf, (leftHeld, leftObjGraspType, leftObjGrasp), \
(rightHeld, rightObjGraspType, rightObjGrasp), objects = makeMLS(test.bs)
set_or_state(env, test.world, conf,
{name: hu.Pose(*pose)
for name, _, _, pose in objects})
raw_input('Finish?')
def make_pick(hand,
obj,
grasp_index,
mu_grasp,
grasp_conf,
approach_conf,
var=tinyVar):
# grasp_index is just for updating discrete distribution over grasps
#grasp_desc = glob.objectDataBase['graspDesc'][obj]
#gb = ObjGraspB(obj, grasp_desc, grasp, None, PoseD(hu.Pose(*mu), var))
grasp_descs = None
gb = ObjGraspB(obj, grasp_descs, grasp_index, None,
PoseD(hu.Pose(*mu_grasp), var))
return ('Pick', (approach_conf, grasp_conf, hand, gb))
def sample_base_pose_from_hand_pose(
robot,
hand_pose,
attempts=20): #Hand pose represents the tip of the hand
base_limits = map(lambda pair: ranges.realRange(*pair),
list(robot.limits(['pr2Base'])))
for i in range(attempts):
base_pose = hand_pose.compose(choice(HORIZONTAL).inversePose()).pose()
if not (base_limits[0].inside(base_pose.x) and base_limits[1].inside(
base_pose.y) and base_limits[2].inside(base_pose.theta)):
continue
base_pose = hu.Pose(base_pose.x, base_pose.y, 0.0, base_pose.theta)
#if not object_collides(base.applyTrans(base_pose), obstacles = self.obstacles):
return base_pose
return None
def pose_from_quat_point(quat, xyz):
angle = euler_from_quaternion(quat)[2] # z rotation
return hu.Pose(xyz[0], xyz[1], xyz[2], angle)
(0.,0.,1.,-0.025),
(1.,0.,0.,0.02),
(0.,0.,0.,1.)]))
gMat0h = hu.Transform(np.array([(0.,1.,0.,0.0),
(0.,0.,1.,-0.025),
(1.,0.,0.,0.05),
(0.,0.,0.,1.)]))
gMat1 = hu.Transform(np.array([(0.,-1.,0.,0.),
(0.,0.,-1.,0.025),
(1.,0.,0.,0.02),
(0.,0.,0.,1.)]))
gMat1h = hu.Transform(np.array([(0.,-1.,0.,0.),
(0.,0.,-1.,0.025),
(1.,0.,0.,0.05),
(0.,0.,0.,1.)]))
gMat4 = hu.Pose(0,0,0,math.pi/2).compose(gMat0)
gMat5 = hu.Pose(0,0,0,-math.pi/2).compose(gMat0)
# from the top
gMat2= hu.Transform(np.array([(-1.,0.,0.,0.),
(0.,0.,-1.,0.025),
(0.,-1.,0.,0.),
(0.,0.,0.,1.)]))
gMat3= hu.Transform(np.array([(1.,0.,0.,0.),
(0.,0.,1.,-0.025),
(0.,-1.,0.,0.),
(0.,0.,0.,1.)]))
gdesc0 = lambda obj: GDesc(obj, gMat0, 0.05, 0.05, 0.025)
gdesc0h = lambda obj: GDesc(obj, gMat0h, 0.05, 0.05, 0.025)
gdesc1 = lambda obj: GDesc(obj, gMat1, 0.05, 0.05, 0.025)
gdesc1h = lambda obj: GDesc(obj, gMat1h, 0.05, 0.05, 0.025)
def grasp_test(env,
no_window=True,
name='objA',
hand='right',
window_name='W'):
exp, goal = test0()
test = PlanTest(exp, noWindow=no_window)
test.load() # TODO - do I need to pass regions here?
belief = test.bs
print SEPARATOR
# NOTE - should cascade functions to get values
# NOTE - I need to be able to generate from the current placement
#domainProbs.maxGraspVar
#graspV = pbs.beliefContext.domainProbs.maxGraspVar
hand = 'left'
grasp_index = 2
inputs = [
'objA', grasp_index, (0, 0, 0, 0), typicalErrProbs.placeVar,
typicalErrProbs.pickVar, typicalErrProbs.placeDelta,
typicalErrProbs.graspDelta, hand, probForGenerators
]
for outputs in pick_fn.fun(inputs, goal.fluents, belief):
print outputs
raw_input('Continue?')
return
print SEPARATOR
conf, _, _, objects = makeMLS(belief) # Extract MLS from b
objects_dict = {props[0]: props[1:] for props in objects}
type_name, support_face, pose = objects_dict[name]
wrapped_pose = hu.Pose(*pose)
grasp_desc = belief.objects[name].attrs[
'graspDesc'] # 1 is side grasp and 3 is top grasp
set_or_state(env, belief.pbs.getWorld(), conf,
{name: hu.Pose(*pose)
for name, _, _, pose in objects})
robot = env.GetRobots()[0]
placeB = belief.pbs.makePlaceB(
name,
support=support_face,
pose=wrapped_pose,
var=tinyVar,
delta=typicalErrProbs.placeDelta) # ObjPlaceB
for grasp_index in range(len(grasp_desc)):
print '\nGrasp index:', grasp_index
graspB = belief.pbs.makeGraspB(
name,
grasp=grasp_index,
support=None,
pose=IDENTITY_POSE,
var=tinyVar,
delta=typicalErrProbs.graspDelta) # ObjGraspB
for conf1, conf2, _ in islice(
potentialGraspConfGen(belief.pbs,
placeB,
graspB,
None,
hand,
None,
prob=0.001,
nMax=100,
findApproach=True), 10):
set_full_config(robot, or_from_hpn_conf(robot, conf1))
robot.SetDOFValues(
np.array([0.54800022]),
robot.GetManipulator(hand + 'arm').GetGripperIndices())
conf1.draw(window_name)
raw_input('One?')
set_full_config(robot, or_from_hpn_conf(robot, conf2))
conf2.draw(window_name)
raw_input('Two?')
def policy(self, belief, goal):
if satisfies(belief, goal):
return []
#if self.iterations != 0:
# return []
self.iterations += 1
if self.plan is not None:
if len(self.plan) == 0:
return []
return [self.plan.pop(0)]
print
print belief.pbs.__class__ # PBS (Planning Belief State?)
print belief.pbs.getWorld().__class__ # World
# TODO - realWorld is RealWorld or RobotEnv within PlanTest. Has executePrim within it
print 'Regions', self.exp.regions
USE_HAND = 'right'
print belief.makeObjGraspB(USE_HAND) # Gets the current grasp?
conf, (leftHeld, leftObjGraspType, leftObjGrasp), \
(rightHeld, rightObjGraspType, rightObjGrasp), objects = makeMLS(belief) # Extract MLS from b
objects_dict = {props[0]: props[1:] for props in objects}
robot = conf.robot
#print conf.cartConf()
print
name = 'objA'
type_name, support_face, pose = objects_dict[name]
wrapped_pose = hu.Pose(*pose)
identity_pose = hu.Pose(0, 0, 0, 0)
print wrapped_pose.matrix
print type_name, support_face, pose
geom, _ = glob.constructor[type_name](name=name)
grasp_desc = belief.objects[name].attrs['graspDesc']
grasp_index = 0
grasp = grasp_desc[grasp_index]
# G * M = P
print grasp.frame # Transform
print grasp.dx, grasp.dy, grasp.dz # Don't know what these do...
grasp_trans = grasp.frame.inverse().compose(wrapped_pose)
print robot.potentialBasePosesGen
print robot.inverseKinWristGen # Allows choosing base pose
print graspConfHypGen
#grasp_cart = RobotCartConf(grasp_trans, robot) # First argument must be a conf?
grasp_cart = conf.getCart()
grasp_cart = grasp_cart.set('pr2RightArm', grasp_trans)
print
print 'Kinematics'
print conf.getCart()
#print conf
#print robot.inverseKin(get_manip(conf), conf)
print robot.inverseKin(grasp_cart, conf)
print
# TODO - need to sample the base?
# Return plan with one move operation
currConf = copy.copy(conf)
goalConf = currConf.setBaseConf((0.2, 0.2, 0.2))
self.plan = [
make_move(currConf, goalConf),
make_pick(USE_HAND, name, grasp_desc, grasp_index, pose, tinyVar,
currConf, currConf),
]
print
print conf
print leftHeld, leftObjGraspType, leftObjGrasp
print rightHeld, rightObjGraspType, rightObjGrasp
print objects
#print goal.satisfies(belief) # Nope
#print belief.satisfies(goal) # Nope
print goal
print self.plan
raw_input('Found plan! Continue?')
print
return None
#return None # Fail
#return [] # Sucess
return self.plan[:1] # NOTE - only the first step really matters
def sample_pose(pose_belief, dist):
face_frame = pose_belief.faceFrames[pose_belief.restFace]
return hu.Pose(*dist.draw()).compose(face_frame.inverse())
if plan is None:
return
print plan
if no_window:
visualize_plan(Plan(
convert_state(oracle, stream_problem.initial_atoms),
executable_plan(oracle, plan)),
oracle,
display=True,
save=None)
#################################################################
IDENTITY_POSE = hu.Pose(0, 0, 0, 0)
from mm_operators.mmOps import PickGen, PlaceGen, probForGenerators, PoseInRegionGen
from mm_operators.mmGen import pickGenTop, potentialGraspConfGen, placeInRegionGenGen, placeInGenTop, potentialRegionPoseGen
# NOTE - Leslie and Tomas's primitives try placing in the belief of something
from mm_operators.mmOps import moveNBPrim, movePrim # These produce motions
# NOTE - potentialGraspConfGen is used for both pick and place (duh)
pick_fn = PickGen([
'Pose', 'PoseFace', 'PickConf', 'PreConf', 'ConfDelta', 'SupportObj',
'SupportObjPoseFace'
], [
'Obj', 'GraspFace', 'GraspMu', 'PoseVar', 'RealGraspVar', 'PoseDelta',
