Controlling individual robot joint using keyboard

In this guide, we will learn how we can control each individual joint of the left arm of the G1 using our keyboard input. Alternatively, you can also replace the keyboard input with any other device you would like such as a controller. This guide only acts us an example of what we could achieve. If you have not gone through the previous setting up guidge, please take sometime to go through that portion first as this guide will build up on we have already completed there.

I. What we have so far

This is the code structure that we have from the previous session, all the comment has been taken out for brievity:

1. Linux

import numpy as np
import genesis as gs
import json
 
gs.init(
    backend=gs.gs_backend.gpu,
    # backend = gs.gs_backend.cpu
    precision="32",
)
 
scene = gs.Scene(
    viewer_options=gs.options.ViewerOptions(
        camera_pos=(3, 0, 2.5),
        camera_lookat=(0.0, 0.0, 0.5),
        camera_fov=40,
        max_FPS=60,
    ),
    sim_options=gs.options.SimOptions(
        dt=0.01,
    ),
    show_viewer=True,
)
 
plane = scene.add_entity(
    gs.morphs.Plane(),
)
 
root_path = "robot_models/unitree/g1/torso_only/"
 
g1 = scene.add_entity(
    gs.morphs.MJCF(
        file=root_path + "g1_dual_arm.xml",
    )
)
 
scene.build()
 
with open(root_path + "desc/g1_dual_arm_xml_config.json", "r") as f:
    model_data = json.load(f)
 
l_arm_jnt_names = [
    "left_shoulder_pitch_joint",
    "left_shoulder_roll_joint",
    "left_shoulder_yaw_joint",
    "left_elbow_joint",
    "left_wrist_roll_joint",
    "left_wrist_pitch_joint",
    "left_wrist_yaw_joint",
]
 
l_arm_dofs_idx = [g1.get_joint(name).dof_idx_local for name in l_arm_jnt_names]
 
g1.set_dofs_kp( 
    kp=np.array([500] * len(l_arm_jnt_names)),
    dofs_idx_local=l_arm_dofs_idx,
)
 
g1.set_dofs_kv( 
    kv=np.array([30] * len(l_arm_jnt_names)),
    dofs_idx_local=l_arm_dofs_idx,
)
 
g1.set_dofs_force_range( 
    lower=np.array(
        [
            model_data["joints"][name]["limits"]["force"][0]
            for name in l_arm_jnt_names
        ]
    ),
    upper=np.array(
        [
            model_data["joints"][name]["limits"]["force"][1]
            for name in l_arm_jnt_names
        ]
    ),
    dofs_idx_local=l_arm_dofs_idx,
)
 
g1.set_dofs_position(
    np.array([0] * 7),
    l_arm_dofs_idx,
)
 
for i in range(200): 
    if i == 100:
        g1.control_dofs_position( 
            np.array([-1, 0, 0, 0, 0, 0, 0]),
            l_arm_dofs_idx,
        )
 
    scene.step()

2. MacOS

import numpy as np
import genesis as gs
import json
 
gs.init(
    backend=gs.gs_backend.gpu,
    # backend = gs.gs_backend.cpu
    precision="32",
)
 
scene = gs.Scene(
    viewer_options=gs.options.ViewerOptions(
        camera_pos=(3, 0, 2.5),
        camera_lookat=(0.0, 0.0, 0.5),
        camera_fov=40,
        max_FPS=60,
    ),
    sim_options=gs.options.SimOptions(
        dt=0.01,
    ),
    show_viewer=True,
)
 
plane = scene.add_entity(
    gs.morphs.Plane(),
)
 
root_path = "robot_models/unitree/g1/torso_only/"
 
g1 = scene.add_entity(
    gs.morphs.MJCF(
        file=root_path + "g1_dual_arm.xml",
    )
)
 
scene.build()
 
with open(root_path + "desc/g1_dual_arm_xml_config.json", "r") as f:
    model_data = json.load(f)
 
l_arm_jnt_names = [
    "left_shoulder_pitch_joint",
    "left_shoulder_roll_joint",
    "left_shoulder_yaw_joint",
    "left_elbow_joint",
    "left_wrist_roll_joint",
    "left_wrist_pitch_joint",
    "left_wrist_yaw_joint",
]
 
l_arm_dofs_idx = [g1.get_joint(name).dof_idx_local for name in l_arm_jnt_names]
 
g1.set_dofs_kp( 
    kp=np.array([500] * len(l_arm_jnt_names)),
    dofs_idx_local=l_arm_dofs_idx,
)
 
g1.set_dofs_kv( 
    kv=np.array([30] * len(l_arm_jnt_names)),
    dofs_idx_local=l_arm_dofs_idx,
)
 
g1.set_dofs_force_range( 
    lower=np.array(
        [
            model_data["joints"][name]["limits"]["force"][0]
            for name in l_arm_jnt_names
        ]
    ),
    upper=np.array(
        [
            model_data["joints"][name]["limits"]["force"][1]
            for name in l_arm_jnt_names
        ]
    ),
    dofs_idx_local=l_arm_dofs_idx,
)
 
 
def run_sim(scene):
    g1.set_dofs_position(
        np.array([0] * 7),
        l_arm_dofs_idx,
    )
 
    for i in range(200): 
        if i == 100:
            g1.control_dofs_position( 
                np.array([-1, 0, 0, 0, 0, 0, 0]),
                l_arm_dofs_idx,
            )
 
        scene.step()
 
 
gs.tools.run_in_another_thread(fn=run_sim, args=(scene,))
 
if scene.visualizer.viewer is not None:
    scene.visualizer.viewer.start()

In this guide we can ignore all the part above g1.set_dofs_position as we virtually won’t make any change to it. For MacOS all the ammendment will be made solely in the run_sim() function only, the rest will stay roughly the same.

II. Installation

First, we need to install a library to handle the input. In this guide, we will be using pynput: t

pip install pynput

And then we will add 2 new imports to the top of our file:

import genesis as gs
import numpy as np
import json
 
#------->
from pynput import keyboard
import time

III. Controlling code

First we define below the g1.set_dofs_position() function the following parameters:

g1.set_dofs_position(np.array([0] * 7), l_arm_dofs_idx)
movement_speed = 0.1  # delta change of the angle value when an input is received
current_positions = np.array([0.0] * 7) # starting position of all the angle
 
with open(root_path + "desc/g1_dual_arm_xml_config.json", "r") as f:
    model_data = json.load(f)
 
# the limits is used to clip value preventing angle from going higher than the limited range
limits = [model_data["joints"][name]["limits"]["angle"] for name in l_arm_jnt_names]
 
# Key mappings for joint control
# Using Q/A, W/S, E/D, R/F, T/G, Y/H, U/J for the 7 joints
key_mappings = {
    "q": (0, movement_speed, limits[0]),  # Joint 1 positive
    "a": (0, -movement_speed, limits[0]),  # Joint 1 negative
    "w": (1, movement_speed, limits[1]),  # Joint 2 positive
    "s": (1, -movement_speed, limits[1]),  # Joint 2 negative
    "e": (2, movement_speed, limits[2]),  # Joint 3 positive
    "d": (2, -movement_speed, limits[2]),  # Joint 3 negative
    "r": (3, movement_speed, limits[3]),  # Joint 4 positive
    "f": (3, -movement_speed, limits[3]),  # Joint 4 negative
    "t": (4, movement_speed, limits[4]),  # Joint 5 positive
    "g": (4, -movement_speed, limits[4]),  # Joint 5 negative
    "y": (5, movement_speed, limits[5]),  # Joint 6 positive
    "h": (5, -movement_speed, limits[5]),  # Joint 6 negative
    "u": (6, movement_speed, limits[6]),  # Joint 7 positive
    "j": (6, -movement_speed, limits[6]),  # Joint 7 negative
}

Next, we will be defining the portion that handle all the keyboard input:

pressed_keys = set()
 
def on_press(key):
    try:
        # Convert key to character if it's alphanumeric
        k = key.char.lower()
        if k in key_mappings:
            pressed_keys.add(k)
    except AttributeError:
        # Handle special keys
        if key == keyboard.Key.space:
            # Reset position
            nonlocal current_positions
            current_positions = np.array([0.0] * 7)
            g1.control_dofs_position(current_positions, l_arm_dofs_idx)
        elif key == keyboard.Key.esc:
            # Stop listener, stop simulation
            scene.visualizer.viewer.stop()
            return False
 
def on_release(key):
    try:
        k = key.char.lower()
        if k in key_mappings:
            pressed_keys.discard(k)
    except AttributeError:
        pass
 
 
 
listener = keyboard.Listener(on_press=on_press, on_release=on_release)
listener.start()

Now that we are done with the keyboard control code, we can go on to define our main loop. It is important to note that different from the previous setting, we let the simulation run in an indefinite loop as long as the listener is alive. This is because our input is not dependent on time.

try:
    while listener.is_alive():
        # Process all currently pressed keys
        # There could be a visible lag from the point of keypressed to action
        # so we define key pressed set for that purpose
        for key in pressed_keys:
            joint_idx, delta, bound = key_mappings[key]
            current_positions[joint_idx] += delta
            # preventing the angle from going over the angle limit
            current_positions[joint_idx] = np.clip(
                current_positions[joint_idx], bound[0], bound[1]
            )
 
        g1.control_dofs_position(current_positions, l_arm_dofs_idx)
 
        scene.step()
 
        # Prevent system overwhelming
        time.sleep(0.01)
 
except KeyboardInterrupt:
    print("\nSimulation terminated by user")

And with that we can now run our simulation by calling:

python main.py

Now, you should be able to see the simulation start rendering. If you press q or a you should be able to see the robot left shoulder moving up and down smoothly as intended. You can also press all the other keys to see how each joint move and help with visualziation of what an angle limit looks like.

IV. Wrapping up

In this guide, we have learnt how to create a small control script to control the robot left arm in Genesis simulation. For the next step, we will learn how to use the built in inverse_kinematic solver to tell the robot hand to move from point A to point B automatically without us having to specify the movement angle of each joint.

Setting up basic environment in Genesis Using kinematic to move arm from point A to B