Esempio n. 1
0
def test_is_gp():
    env = is_gp_test_env()

    hl_plan = HLPlan(env)
    robot = Robot(env.GetRobots()[0].GetName())
    robot_pose = np.array([[0.1],[0.],[0.]])
    robot.set_pose(env, robot_pose)

    # obj has a radius of .35
    obj = Obj('obj')
    pose = np.array([[0.],[0.],[0.]])
    obj.set_pose(env, pose)

    pos = HLParam("pos", (3, 1), is_var=False, value=robot.get_pose(env))
    obj_pos = HLParam("obj_pos", (3, 1), is_var=False, value=obj.get_pose(env))
    gp = HLParam("gp", (3, 1), is_var=False, value=pose)
    action = TestIsGPAction(hl_plan, env, robot, obj, gp, pos, obj_pos)
    fluent = action.precondition
    fluent.pre()
    raw_input('')
    poses = [np.array([[0.7],[0.],[0.]]), \
            np.array([[0.65],[0.],[0.]]), \
            np.array([[0.55],[0.],[0.]]), \
            np.array([[0.45],[0.],[0.]])]
    values = [0., -0.05, 0.05, 0.15]
    grads = [np.array([[0., 0., 0.]]),
            np.array([[-1.0 ,0., 0.]]), \
            np.array([[-1.0 ,0., 0.]]), \
            np.array([[-1.0 ,0., 0.]])]
    for i in range(len(poses)):
        val, grad = fluent.distance_from_obj(poses[i], 0.05, (3,1))
        print 'val: ', val
        print 'grad: ', grad
        import ipdb; ipdb.set_trace()
Esempio n. 2
0
def test_pick_and_move():
    env = pick_test_env()

    # start = HLParam("start", (3, 1), is_var=False, value=np.array([[-2], [0], [0]]))
    # end = HLParam("end", (3, 1), is_var=False, value=np.array([[2], [0], [0]]))

    hl_plan = TestDomain(env)
    pick_env = env.CloneSelf(1) # clones objects in the environment
    move_env = env.CloneSelf(1) # clones objects in the environment

    robot = Robot(env.GetRobots()[0].GetName())
    rp = HLParam("rp", (3, 1))
    end = HLParam("end", (3, 1), is_var=False, value=np.array([[2],[0],[0]]))
    gp = GP("gp", (3, 1), is_resampled=True)
    obj = Obj("obj")
    obj_loc = HLParam("obj_loc", (3, 1), is_var=False, value=obj.get_pose(env))

    gp.resample()

    pick = Pick(0, hl_plan, pick_env, robot, rp, obj, obj_loc, gp)
    move = Move(0, hl_plan, move_env, robot, rp, end, obj, gp)
    hlas = [pick, move]

    ll_prob = LLProb(hlas)
    ll_prob.solve()

    ipdb.set_trace()
Esempio n. 3
0
def test_pick_move_and_place():
    env = pick_test_env()
    robot = Robot(env.GetRobots()[0].GetName())

    hl_plan = TestDomain(env)
    place_env = env.CloneSelf(1) # clones objects in the environment
    pick_env = env.CloneSelf(1) # clones objects in the environment
    move_env = env.CloneSelf(1) # clones objects in the environment

    rp1 = HLParam("rp1", (3, 1))
    rp2 = HLParam("rp2", (3, 1))
    gp = GP("gp", (3, 1), is_resampled=True)
    obj = Obj("obj")
    obj_loc = HLParam("obj_loc", (3, 1), is_var=False, value=obj.get_pose(env))
    target_loc = HLParam("target_loc", (3, 1), is_var=False, value=np.array([[2],[0.5],[0]]))

    gp.resample()

    pick = Pick(0, hl_plan, pick_env, robot, rp1, obj, obj_loc, gp)
    move = Move(0, hl_plan, move_env, robot, rp1, rp2, obj, gp)
    import ipdb; ipdb.set_trace()
    place = Place(0, hl_plan, place_env, robot, rp2, obj, target_loc, gp)
    hlas = [pick, move, place]

    ll_prob = LLProb(hlas)
    ll_prob.solve()
Esempio n. 4
0
def test_pick():
    env = pick_test_env()

    hl_plan = TestDomain(env)
    pick_env = env.CloneSelf(1) # clones objects in the environment

    robot = Robot(env.GetRobots()[0].GetName())
    rp = HLParam("rp", (3, 1))

    # collision checking returns -1 * normal when objects are touching
    gp_val =  np.array([[0], [0.56], [0]], dtype=np.float)
    # gp_val =  np.array([[0], [0.55], [0]], dtype=np.float)
    # gp_val =  np.array([[0], [0.54], [0]], dtype=np.float)
    # gp_val =  np.array([[0], [0.552], [0]], dtype=np.float)
    gp = GP("gp", (3, 1), value=gp_val, is_resampled=True)
    obj = Obj("obj")
    obj_loc = HLParam("obj_loc", (3, 1), is_var=False, value=obj.get_pose(env))

    # gp.resample()

    pick = Pick(0, hl_plan, pick_env, robot, rp, obj, obj_loc, gp)
    hlas = [pick]

    ll_prob = LLProb(hlas)
    ll_prob.solve()
    assert np.allclose(pick.pos.value, np.array([[-2.0],[-0.61],[ 0.]]))
Esempio n. 5
0
def test_not_obstructs_btn_two_cylinders():
    env = not_obstructs_test_env()

    hl_plan = HLPlan(env)
    robot = Robot(env.GetRobots()[0].GetName())
    robot_pose = np.array([[0.1],[0.],[0.]])
    robot.set_pose(env, robot_pose)

    # obj has a radius of .35
    obj = Obj('obj')
    pose = np.array([[0.],[0.],[0.]])
    obj.set_pose(env, pose)

    pos = HLParam("pos", (3, 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()
    raw_input('')
    poses = [np.array([[0.7],[0.],[0.]]), \
            np.array([[0.65],[0.],[0.]]), \
            np.array([[0.55],[0.],[0.]]), \
            np.array([[0.45],[0.],[0.]])]
    values = [0., -0.05, 0.05, 0.15]
    grads = [np.array([[0., 0., 0.]]),
            np.array([[-1.0 ,0., 0.]]), \
            np.array([[-1.0 ,0., 0.]]), \
            np.array([[-1.0 ,0., 0.]])]
    for i in range(len(poses)):
        val, grad = fluent.collisions(poses[i])
        print 'val: ', val
        print 'grad: ', grad
        assert np.allclose(values[i], val, atol=2e-2)
        assert np.allclose(grads[i], grad, atol=2e-2)