robot_tutorials/tutorial_1/tutorial_1_solution.py at main · Wisc-HCI/robot_tutorials · 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
112
113
import time
import threading
import argparse
from pathlib import Path

import numpy as np

from robot_motion_interface.interface import Interface
from robot_motion_interface.mujoco.mujoco_browser import MujocoBrowserInterface
from robot_motion_interface.bimanual_interface import BimanualInterface


def configure_interface(interface_str:str) -> Interface:
    """
    Configure the robot interface based on real or sim

    Args:
        interface_str (str): Either "sim" or "real"
    Returns:
        (Interface) The specific child interface (MujocoBrowserInterface, BimanualInterface)
    """

    config_dir = Path(__file__).resolve().parents[1] / "libs" / "robot-stack" / "robot_motion_interface" / "config"

    if (interface_str == "sim"):
        config_path = config_dir / "mujoco_config.yaml"
        interface = MujocoBrowserInterface.from_yaml(config_path)

    elif (interface_str == "real"):
        config_path = config_dir / "bimanual_arm_config.yaml"
        interface = BimanualInterface.from_yaml(config_path)

    else:
        raise ValueError(f"Unsupported interface: {interface_str}")

    return interface


def move_right_arm(robot_interface:Interface, cartesian_pose:np.ndarray):
    """
    This moves the robot at a set velocity.
    I defined this helper function to reduce code duplication.

    Args:
        robot_interface (Interface): The robot interface (sim or real).
        cartesian_pose (np.ndarray): Target pose [x, y, z, qx, qy, qz, qw] in meters/radian
    """
    goal_poses = [np.array(cartesian_pose)]
    trajectories, _ = robot_interface.cartesian_trajectory(
        goal_poses=goal_poses, dt=0.01, velocity=0.005, angular_velocity=0.005,
        acceleration=0.005, ee_frames=["right_delto_offset_link"])

    for waypoint in trajectories[0]:
        robot_interface.set_cartesian_pose(x_list=[waypoint],
            ee_frames=["right_delto_offset_link"], blocking=True)
    print("Finished executing waypoints.")


def main(interface_str:str):
    """
    One solution to picking up a block either in real
    or sim.

    Args:
        interface_str (str): Either "sim" or "real"
    """

    robot_interface = configure_interface(interface_str)

    # Start the simulation/control loop
    robot_interface.start_loop()
    robot_interface.home(blocking=True)


    ########################################################
    #   This is just one solution, there are countless!
    #########################################################

    # 1. Move above the cube
    move_right_arm(robot_interface, np.array([0.1, 0.0, 1.3, 0.0, 1.0, 0.0, 0.0]))

    # 2. Move down to cube
    move_right_arm(robot_interface, np.array([0.1, 0.0,  1.0, 0.0, 1.0, 0.0, 0.0]))

    # 4. Move cube Left
    move_right_arm(robot_interface, np.array([-0.1, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0]))

    # 5. Move Up
    move_right_arm(robot_interface, np.array([0.0, 0.0, 1.2, 0.0, 1.0, 0.0, 0.0]))


    ########################################################
    ########################################################


    # Continuously loop to keep sim up
    try:
        while(True):
            time.sleep(0.1)
    except (KeyboardInterrupt):
        print("\nStopping Interface.")
    finally:
        robot_interface.stop_loop()


if __name__ == "__main__":
    parser = argparse.ArgumentParser(description="Run joint oscillation demo for Panda or Isaacsim.")
    parser.add_argument("--interface", type=str, choices=["sim", "real"], required=True)

    args = parser.parse_args()
    main(args.interface)