def test_place():
env = pr2_small_cans_test_env()
# robot = PR2('pr2')
# active_body_parts = ['rightarm', 'rgripper', 'torso', 'base']
# active_bodyparts = ['rightarm', 'rgripper', 'base']
active_bodyparts = ["rightarm", "rgripper"]
robot = PR2("pr2", active_bodyparts)
robot.tuck_arms(env)
hl_plan = TestDomain(env)
place_env = env.CloneSelf(1) # clones objects in the environment
robot.init_for_env(env)
robot.init_for_env(place_env)
pose = robot.get_pose(env)
robot_body = robot.get_env_body(env)
hl_plan = HLPlan(env)
obj = Obj("object7")
K = robot.dofs
# obj_pose = np.array([[0.],[0.],[0.]])
# obj.set_pose(env, obj_pose)
# start = HLParam("pos", self.(K, 1), is_var=False, value=robot.get_pose(env))
# pose = np.array([[-1.57265580e+00], [5.27875957e-01], [-1.40000000e+00], \
# [-1.58244363e+00], [-1.64143184e+00], [-9.76938766e-02], [-5.78403045e-01], [0.0],[0.],[0.],[0.]])
# pose = np.array([[-1.57265580e+00], [5.27875957e-01], [-1.40000000e+00], \
# [-1.58244363e+00], [-1.64143184e+00], [-9.76938766e-02], [-5.78403045e-01]])
pose = np.array(
[
[-1.57265580e00],
[5.27875957e-01],
[-1.40000000e00],
[-1.58244363e00],
[-1.64143184e00],
[-9.76938766e-02],
[-5.78403045e-01],
[0.4],
]
)
start = HLParam("start", (K, 1), value=pose, is_var=False)
end = HLParam("end", (K, 1), value=pose)
loc = HLParam("loc", (3, 1), is_var=False, value=obj.get_pose(env)[3:6].reshape((3, 1)))
# import ipdb; ipdb.set_trace()
place = PR2Place(0, hl_plan, place_env, robot, start, end, obj, loc)
hlas = [place]
ll_prob = LLProb(hlas)
# ll_prob.solve()
import ipdb
ipdb.set_trace()
ll_prob.solve_at_priority(-1)
ll_prob.solve_at_priority(1)
def test_move_pr2_gradient():
env = cans_world_env()
obj17 = env.GetKinBody('object17')
tran = obj17.GetTransform()
tran[0,3] = .49268
tran[1,3] = -.51415
obj17.SetTransform(tran)
hl_plan = TestDomain(env)
move_env = env.CloneSelf(1) # clones objects in the environment
move_pr2 = move_env.GetKinBody('pr2')
move_pr2.SetActiveDOFs(move_pr2.GetManipulator('rightarm').GetArmIndices())
move_pr2.SetDOFValues([0.3],
[move_pr2.GetJoint("torso_lift_joint").GetDOFIndex()])
robot = PR2('pr2')
robot.tuck_arms(env)
robot._set_active_dofs(env, ['rightarm'])
pose = robot.get_pose(env)
hl_plan = HLPlan(env)
obj = Obj('object17')
K = 7
# obj_pose = np.array([[0.],[0.],[0.]])
# obj.set_pose(env, obj_pose)
# start = HLParam("pos", (self.K, 1), is_var=False, value=robot.get_pose(env))
start_pose = np.array([[-1.57265580e+00], [5.27875957e-01], [-1.40000000e+00], \
[-1.58244363e+00], [-1.64143184e+00], [-9.76938766e-02], [-5.78403045e-01]])
end_pose = np.array([[-1.31066711e+00], [1.08996543e+00], [-1.40000000e+00],\
[-2.10243169e+00], [2.93131497e+00], [-6.18669215e-01], [5.20253411e-01]])
start = HLParam("start", (K, 1), is_var=False, value=start_pose)
end = HLParam("end", (K, 1), is_var=False, value=end_pose)
obj_pos = HLParam("obj_pos", (3, 1), is_var=False, value=obj.get_pose(env))
move = PR2Move(0, hl_plan, move_env, robot, start, end, obj)
hlas = [move]
ll_prob = LLProb(hlas)
ll_prob.solve()
import ipdb; ipdb.set_trace()
def test_not_obstructs_pr2_table():
env = cans_world_env()
robot = PR2('pr2')
robot.tuck_arms(env)
robot._set_active_dofs(env, ['rightarm', 'torso', 'base'])
# pose = np.array([-0.023593, 1.10728, -1.5566882, -2.124408, -1.4175, \
# -1.8417, 0.21436, 0., 0.2, 0., -0.])
# robot.set_pose(env, pose)
pose = robot.get_pose(env)
hl_plan = HLPlan(env)
obj = Obj('object9')
# obj_pose = np.array([[0.],[0.],[0.]])
# obj.set_pose(env, obj_pose)
pos = HLParam("pos", (11, 1), is_var=False, value=robot.get_pose(env))
obj_pos = HLParam("obj_pos", (3, 1), is_var=False, value=obj.get_pose(env))
action = TestAction(hl_plan, env, robot, pos, obj)
fluent = action.precondition
fluent.pre()
val, grad = fluent.collisions(pose)
