Ejemplo 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()
Ejemplo n.º 2
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)