IALP-Code/test.py at main · nicehiro/IALP-Code · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
from LMs.gpt import GPTAssistant, GPT
from LMs.gemini import GeminiPro
from utils.llm_utils import get_env_variable
from runner import Runner
from robots.tiago import Tiago
from scipy.spatial.transform import Rotation as R


def test_gemini():
    gemini_project_id = get_env_variable("GEMINI_PROJECT_ID")
    gemini_location = get_env_variable("GEMINI_DISTRICT")
    gemini = GeminiPro(gemini_project_id, gemini_location, model="gemini-1.5-flash-001")

    prompt = "describe the image"
    image_path = "./LMs/t1.jpg"
    prompt = gemini.create_prompt(prompt, image_paths=[image_path])
    print(gemini.generate(prompt))


def test_openai():
    azure_openai_key = get_env_variable("AZURE_OPENAI_KEY")
    azure_endpoint = get_env_variable("AZURE_OPENAI_ENDPOINT")

    gpt = GPTAssistant(
        azure_openai_key=azure_openai_key,
        endpoint=azure_endpoint,
        model_name="gpt-4o",
    )
    with open("prompts/assistant/system.txt", "r") as f:
        system_msg = f.read()
    gpt.create_prompt(system_message=system_msg)
    image_path = "./LMs/t1.jpg"
    r = gpt.generate(message="describe the image", image_path=image_path)
    # print(r)
    with open("prompts/assistant/generate_init.txt", "r") as f:
        query = f.read()
    query = query.replace("[INSTRUCTION]", "take the pill box in the drawer to me")
    query = query.replace("[OBJECTS]", "pill_box, drawer, me")
    # print(query)
    r = gpt.generate(message=query, clear_conversation=True)
    print(r)


def test_openai_2():
    azure_openai_key = get_env_variable("AZURE_OPENAI_KEY")
    azure_endpoint = get_env_variable("AZURE_OPENAI_ENDPOINT")

    gpt = GPT(
        azure_openai_key=azure_openai_key,
        endpoint=azure_endpoint,
        model_name="gpt-35",
    )

    gpt.create_prompt()
    a, s = gpt.generate(
        instruction="What is the weather?",
        observation="It is raining.",
        action="Bring an umbrella.",
    )

    print(a)
    print(s)


def test_runner():
    runner = Runner()
    runner.init_assistant()
    runner.init_real(
        x1=1.478624,  # -0.085774,  # 1.543999,
        y1=-0.103747,  # 0.864666,  # 0.255889,
        x2=1.778845,  # -0.207002,  # 1.785797,
        y2=-0.133040,  # 1.165950,  # 0.464550,
        # nav_ip="127.0.0.1",
        nav_ip="10.11.135.238",
        # nav_ip="10.68.0.129",
        nav_port=5555,
        manip_ip="10.11.135.238",
        # manip_ip="10.68.0.129",
        manip_port=12340,
    )
    instruction = (
        "Pickup the paper box on the wooden table and place it on the black table."
    )
    runner.instruct_augment(instruction=instruction, robot=None)


def test_tiago():
    tiago = Tiago()

    tiago.init_torso()
    tiago.init_arm()
    tiago.init_gripper()
    tiago.init_head()
    tiago.init_camera()
    tiago.init_solver()

    # tiago._reset_arm()

    p = [0.6, 0.1, 0.9]

    q = R.from_euler("xyz", [0, -90, 0], degrees=True).as_quat()
    tiago._move_by_ik_solver(p, q)


if __name__ == "__main__":
    # test_gemini()
    # test_openai()
    # test_runner()
    # test_tiago()

    test_openai()