예제 #1
0
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)
예제 #2
0
파일: test_pr2.py 프로젝트: c-l/planopt
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()
예제 #3
0
파일: test_pr2.py 프로젝트: c-l/planopt
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)