Run JoyLo Teleoperation#

Before running JoyLo, ensure that the JoyLo is assembled following Step-by-Step Assembly Guidance and calibrated following Setup and Calibration. Also ensure that the robot can be controlled following Run Real Robots.

Simulation Teleoperation#

We first run JoyLo teleoperation in simulation to get familiar with the controls. Start from the brs_ctrl root directory, and run the following command:

python3 scripts/joylo/sim_joylo.py

Now let’s go through what the script does. First we import the necessary libraries and modules:

1 import os
2 import time
3 import pybullet as pb
4 import numpy as np
5
6 from brs_ctrl.asset_root import ASSET_ROOT
7 from brs_ctrl.joylo import JoyLoController
8 from brs_ctrl.joylo.joylo_arms import JoyLoArmPositionController
9 from brs_ctrl.joylo.joycon import R1JoyConInterface

We then define the joint positions corresponding to the neutral pose of the robot with arms relaxed and hanging by the sides.

12 neutral_left_arm_qs = np.array([1.56, 2.94, -2.54, 0, 0, 0])
13 neutral_right_arm_qs = np.array([-1.56, 2.94, -2.54, 0, 0, 0])

The reason we do this is JoyLo internally maps delta joint positions to the robot. In this way, we bypass the need to calibrate the absolute joint positions. All we need to do is to make sure JoyLo arms start from roughly the same position as the robot arms, as shown in the figure below.

../../_images/joylo_neutral_pose.png

Now we instantiate the JoyLo arms interface.

17 joylo_arms = JoyLoArmPositionController(
18     left_motor_ids=[0, 1, 2, 3, 4, 5, 6, 7],
19     right_motor_ids=[8, 9, 10, 11, 12, 13, 14, 15],
20     motors_port="/dev/tty_joylo",
21     left_arm_joint_signs=[-1, -1, 1, 1, 1, 1],
22     right_arm_joint_signs=[-1, -1, 1, 1, -1, 1],
23     left_slave_motor_ids=[1, 3],
24     left_master_motor_ids=[0, 2],
25     right_slave_motor_ids=[9, 11],
26     right_master_motor_ids=[8, 10],
27     left_arm_joint_reset_positions=neutral_left_arm_qs,
28     right_arm_joint_reset_positions=neutral_right_arm_qs,
29     multithread_read_joints=True,
30 )

Here we explain a few important parameters.

left_motor_ids and right_motor_ids are the unique Dynamixel motor IDs of the left and right arms, respectively. They can be assigned using the Dynamixel Wizard. motors_port is the serial port of the U2D2 connected to the robot. We set it to a fixed alias as described in Setup and Calibration. left_arm_joint_signs and right_arm_joint_signs are the signs of the joint angles. You may need to flip the signs if the joints move in the opposite direction.

master_motor_ids and slave_motor_ids are for joints where two Dynamixel motors are used to drive a single joint. In our case, they are the shoulder roll and upper-arm joints. Joint positions will be read from master motors. While both master and slave motors will be driven to the same position. For convenience, we set motors with smaller IDs as master motors.

Now we instantiate the JoyCon interface and finally create the JoyLo interface.

31 joycon = R1JoyConInterface()
32 joylo = JoyLoPositionController(joycon=joycon, joylo_arms=joylo_arms)

Next, we create a PyBullet simulation.

34 pb_client = pb.connect(pb.GUI)
35 pb.setGravity(0, 0, -9.8)
36 # load robot
37 robot = pb.loadURDF(
38     os.path.join(ASSET_ROOT, "robot/r1_pro/r1_pro.urdf"),
39     [0, 0, 0],
40     useFixedBase=True,
41 )
42
43 # reset all joints to 0
44 for i in range(pb.getNumJoints(robot)):
45     pb.resetJointState(robot, i, 0)
46
47 torso_joint_idxs = [6, 7, 8, 9]
48 left_arm_joint_idxs = [15, 16, 17, 18, 19, 20]
49 right_arm_joint_idxs = [30, 31, 32, 33, 34, 35]
50
51 curr_torso_qs = np.array([0.0, 0.0, 0.0, 0.0])

The teleoperation runs in a loop. Note that we only control arms and the torso in simulation for demonstration purposes.

53 while pb.isConnected():
54     joylo_actions = joylo.act(curr_torso_qs)
55
56     for i, q in zip(torso_joint_idxs, joylo_actions["torso_cmd"]):
57         pb.resetJointState(robot, i, q)
58     for i, q in enumerate(joylo_actions["arm_cmd"]["left"]):
59         pb.resetJointState(robot, left_arm_joint_idxs[i], q)
60     for i, q in enumerate(joylo_actions["arm_cmd"]["right"]):
61         pb.resetJointState(robot, right_arm_joint_idxs[i], q)
62
63     pb.stepSimulation()
64
65     curr_torso_qs = []
66     for idx in torso_joint_idxs:
67         curr_torso_qs.append(pb.getJointState(robot, idx)[0])
68     curr_torso_qs = np.array(curr_torso_qs)
69     time.sleep(0.05)

Real Robot Teleoperation#

Now we run JoyLo teleoperation on the real robot. Open a new terminal and run the following command (set the proper ROS_IP and ROS_MASTER_URI if necessary):

export ROS_IP=10.0.0.10  # this makes sure ROS topics can be addressed properly
export ROS_MASTER_URI=http://10.0.0.10:11311
python3 scripts/joylo/real_joylo.py

The real-robot teleoperation script is similar to the simulation one. We explain several key points here. First we create the robot interface as introduced in Run Real Robots.

18 robot = R1Interface(
19     left_gripper=GalaxeaR1Gripper(left_or_right="left", gripper_close_stroke=1),
20     right_gripper=GalaxeaR1Gripper(left_or_right="right", gripper_close_stroke=1),
21 )

Next, we create the JoyCon interface:

22 joycon = R1JoyConInterface(
23     ros_publish_functional_buttons=True,
24     init_ros_node=False,
25     gripper_toggle_mode=True,
26 )

Here, setting ros_publish_functional_buttons to True enables us using the functional buttons on the JoyCon to facilitate the data collection (e.g., start the data collection, discard the data, and save the data). We set init_ros_node to False to avoid creating a new ROS node, since we already have created one in the robot interface. Setting gripper_toggle_mode to True allows us to control the grippers opening and closing by just pressing the triggers once. If set to False, we keep pressing the triggers to close the grippers.

Next, we keep running the control in a loop.

46 alpha = 0.95
47 left_joylo_q = None
48 right_joylo_q = None
49
50 pbar = tqdm()
51 try:
52     while not rospy.is_shutdown():
53         joylo_arms_q = joylo.q
54         left_joylo_q = (
55             joylo_arms_q["left"]
56             if left_joylo_q is None
57             else (1 - alpha) * left_joylo_q + alpha * joylo_arms_q["left"]
58         )
59         right_joylo_q = (
60             joylo_arms_q["right"]
61             if right_joylo_q is None
62             else (1 - alpha) * right_joylo_q + alpha * joylo_arms_q["right"]
63         )
64         curr_torso_qs = robot.last_joint_position["torso"]
65         joycon_action = joycon.act(curr_torso_qs)
66         robot_torso_cmd = np.zeros((4,))
67         robot_torso_cmd[:] = joycon_action["torso_cmd"][:]
68
69         robot.control(
70             arm_cmd={
71                 "left": left_joylo_q,
72                 "right": right_joylo_q,
73             },
74             gripper_cmd={
75                 "left": joycon_action["gripper_cmd"]["left"],
76                 "right": joycon_action["gripper_cmd"]["right"],
77             },
78             torso_cmd=robot_torso_cmd,
79             base_cmd=joycon_action["mobile_base_cmd"],
80         )
81         pbar.update(1)

Notice that the alpha sets an exponential moving average to smooth the trajectory. On the real robot, we can control the arms, grippers, torso, and mobile base, all at the same time.

Bilateral Teleoperation#

JoyLo also supports bilateral teleoperation. At a high level, the JoyLo arms serve as the leader, issuing commands to the robot arms while simultaneously being regularized by the robot’s current joint positions. In this way, JoyLo provides haptic feedback without requiring additional force sensors. This feedback discourages abrupt user motions and offers proportional resistance when the robot experiences contact.

To run bilateral teleoperation, first check the JoyLo arms can move to specified positions. Run the following script to command JoyLo arms to a goal position. They will stay at the position despite external forces.

python3 scripts/joylo/joylo_move.py

Once you are confident that the JoyLo arms can move to the desired positions, run the bilateral teleoperation script:

python3 scripts/joylo/real_joylo_bilateral.py

Now you can control the robot with JoyLo. In the meantime, you can feel the resistance from JoyLo arms. This is because the JoyLo arms are also regularized by the robot arms’ current joint positions.

Let’s explain the bilateral teleoperation script. First we import the necessary libraries and modules as before. But this time we import JoyLoArmImpedanceController instead of JoyLoArmPositionController to achieve impedance control of JoyLo arms.

 1import time
 2import rospy
 3import numpy as np
 4from tqdm import tqdm
 5
 6from brs_ctrl.joylo import JoyLoController
 7from brs_ctrl.joylo.joylo_arms import JoyLoArmImpedanceController
 8from brs_ctrl.joylo.joycon import R1JoyConInterface
 9from brs_ctrl.robot_interface import R1Interface
10from brs_ctrl.robot_interface.grippers import GalaxeaR1Gripper

We create the robot interface and JoyCon interface as before. When we create the JoyLoArmImpedanceController, we need to specify the proportional gains Kp and derivative gains Kd for the impedance control.

28 joylo_arms = JoyLoArmImpedanceController(
29     left_motor_ids=[0, 1, 2, 3, 4, 5, 6, 7],
30     right_motor_ids=[8, 9, 10, 11, 12, 13, 14, 15],
31     motors_port="/dev/tty_joylo",
32     left_arm_joint_signs=[-1, -1, 1, 1, 1, 1],
33     right_arm_joint_signs=[-1, -1, 1, 1, -1, 1],
34     left_slave_motor_ids=[1, 3],
35     left_master_motor_ids=[0, 2],
36     right_slave_motor_ids=[9, 11],
37     right_master_motor_ids=[8, 10],
38     left_arm_Kp=[0.5, 0.5, 0.5, 0.5, 0.5, 0.5],
39     left_arm_Kd=[0.01, 0.01, 0.01, 0.01, 0.01, 0.01],
40     right_arm_Kp=[0.5, 0.5, 0.5, 0.5, 0.5, 0.5],
41     right_arm_Kd=[0.01, 0.01, 0.01, 0.01, 0.01, 0.01],
42     left_arm_joint_reset_positions=neutral_left_arm_qs,
43     right_arm_joint_reset_positions=neutral_right_arm_qs,
44 )

Notice that Kp and Kd need to be tuned case by case. We set them to 0.5 and 0.01 respectively as a starting point.

Next, we start the bilateral teleoperation loop. The key difference is we not only send control commands to the robot but also set goals for JoyLo arms.

56 while not rospy.is_shutdown():
57     joylo_arms_q = joylo.q
58     left_robot_arm_q = robot.last_joint_position["left_arm"]
59     right_robot_arm_q = robot.last_joint_position["right_arm"]
60     left_joylo_q = (
61         joylo_arms_q["left"]
62         if left_joylo_q is None
63         else (1 - alpha) * left_joylo_q + alpha * joylo_arms_q["left"]
64     )
65     right_joylo_q = (
66         joylo_arms_q["right"]
67         if right_joylo_q is None
68         else (1 - alpha) * right_joylo_q + alpha * joylo_arms_q["right"]
69     )
70     curr_torso_qs = robot.last_joint_position["torso"]
71     joycon_action = joycon.act(curr_torso_qs)
72     robot_torso_cmd = np.zeros((4,))
73     robot_torso_cmd[:] = joycon_action["torso_cmd"][:]
74
75     joylo.set_new_arm_goal(
76         {
77             "left": left_robot_arm_q,
78             "right": right_robot_arm_q,
79         }
80     )
81     if not control_started:
82         joylo.start_arm_control()
83         control_started = True
84     robot.control(
85         arm_cmd={
86             "left": left_joylo_q,
87             "right": right_joylo_q,
88         },
89         gripper_cmd={
90             "left": joycon_action["gripper_cmd"]["left"],
91             "right": joycon_action["gripper_cmd"]["right"],
92         },
93         torso_cmd=robot_torso_cmd,
94         base_cmd=joycon_action["mobile_base_cmd"],
95     )
96     pbar.update(1)

Finally, as a fun bonus, we can run the following script to see the robot unilaterally controls the JoyLo arms.

python3 scripts/joylo/r1_to_joylo.py