Method for Teleoperating Any Robotic Arm via Pika

Hi everyone,

I'd like to share a universal method for teleoperating robotic arms using Pika Sense. This approach works with any ROS-enabled robotic arm (we've tested it with Piper, xArm, and UR robots) by leveraging high-precision 6D pose tracking (0.3mm accuracy) and incremental control algorithms. The system publishes standard geometry_msgs/PoseStamped messages on the pika/pose topic, making integration straightforward. Hope this helps anyone looking to implement teleoperation across different robot platforms!

Teleoperation

Teleoperation of robotic arms is achieved using Pika Sense. When used with external positioning base stations, Pika Sense can acquire 6D pose data with an accuracy of 0.3mm. After aligning the coordinate system of Pika Sense with that of the robotic arm's end effector, incremental control is employed to map the 6D pose data to the end effector of the robotic arm, thereby realizing teleoperation.

In summary, the teleoperation principle consists of four key steps:

• Acquire 6D pose data
• Align coordinate systems
• Implement incremental control
• Map 6D pose data to the robotic arm

Below is a detailed breakdown and explanation of each step.

Acquiring 6D Pose Data

Positioning Principle of Pika Sense and Station

1. Positioning Mechanism of Base Stations

Each base station is equipped with an infrared LED array and two rotating laser transmitters (responsible for horizontal and vertical scanning, respectively):

• The infrared LEDs flash globally at a frequency of 60Hz, providing synchronization signals for the entire space.
• The laser transmitters, driven by motors to rotate, emit horizontal and vertical laser beams alternately, scanning the space in a cycle of 10ms (resulting in a complete cycle of 20ms).
• A single base station can achieve a laser scanning coverage of 5×5 meters; with four base stations working collaboratively, the coverage can be expanded to 10×10 meters.

2. Positioning Implementation of Pika Sense

The upper sensor of Pika Sense is called the Tracker, which is densely equipped with more than 70 photosensors on its surface. Each sensor can receive infrared signals and laser scans.

Positioning calculation process:

• The sensors record the time of arrival of the laser, and combined with the base station's scanning cycle, calculate the horizontal and pitch angles of the sensors relative to the base station.
• Through the spatial distribution and time difference data of multiple sensors (≥5), the precise position and orientation of the Tracker are solved.
• Calculations are completed directly by the local processor without the need for image processing, resulting in a delay of only 20ms and a positioning accuracy of 0.3mm.

The 6D pose data is published as messages of the geometry_msgs/PoseStamped type to the pika/pose topic, which is compatible with end pose control of most robotic arms available on the market.

In addition to the ROS message type, if you need to access 6D pose data independent of ROS, please refer to our pika_sdk.

Coordinate System Alignment

In the first step [Acquiring 6D Pose Data], whether the 6D pose data is obtained by subscribing to the ROS topic or via the Pika SDK, the coordinate system of Pika Sense is centered at the gripper, with the x-axis facing forward, the y-axis facing left, and the z-axis facing upward, as shown in the figure below:

Different robotic arms have different coordinate systems for their end effectors. However, for most of them, the z-axis faces forward, while the orientations of the x-axis and y-axis depend on the initial rotation values of the robotic arm's end effector. The method for checking the coordinate system of a robotic arm's end effector varies by model; typically, it can be viewed through the host-software provided by the manufacturer or by loading the robotic arm model in ROS RViz.

After understanding the coordinate systems of both Pika Sense and the robotic arm's end effector, the 6D pose data of Pika Sense is converted into a homogeneous transformation matrix. This matrix is then multiplied by an adjustment matrix to align the Pika Sense coordinate system with the robotic arm's end effector coordinate system. This completes the coordinate system alignment process.

Incremental Control

In the second step [Coordinate System Alignment], we align the coordinate system of Pika Sense with that of the robotic arm's end effector (with the z-axis facing forward). However, a question arises: when holding Pika Sense and moving it forward, will the value of its z-axis necessarily increase positively?

Not necessarily. The pose value is related to its base_link. If the z-axis of base_link is exactly consistent with the z-axis direction of Pika Sense, then the z-axis value of Pika Sense will indeed increase positively. However, the base_link of Pika Sense is a coordinate system generated when Pika Sense is calibrated with the base station, where the x-axis faces forward, the y-axis faces left, and the z-axis faces upward. In other words, base_link is generated randomly.

So, how do we map the coordinates of Pika Sense to the robotic arm's end effector? How can we ensure that when Pika Sense moves forward/left, the robotic arm's end effector also moves forward/left accordingly?

The answer is: use incremental control.

In teleoperation, the pose provided by Pika Sense is an absolute pose. However, we do not want the robotic arm to jump directly to this absolute pose. Instead, we want the robotic arm to follow the relative movement of the operator, starting from its current position. Simply put, it involves converting the absolute pose change of the operating device (Pika Sense) into a relative pose command that the robotic arm needs to execute.

The core code for this functionality is as follows:

# 增量式控制
def calc_pose_incre(self,base_pose, pose_data):
    begin_matrix = tools.xyzrpy2Mat(base_pose[0], base_pose[1], base_pose[2],
                                                base_pose[3], base_pose[4], base_pose[5])
    zero_matrix = tools.xyzrpy2Mat(self.initial_pose_rpy[0],self.initial_pose_rpy[1],self.initial_pose_rpy[2],
                                        self.initial_pose_rpy[3],self.initial_pose_rpy[4],self.initial_pose_rpy[5])
    end_matrix = tools.xyzrpy2Mat(pose_data[0], pose_data[1], pose_data[2],
                                            pose_data[3], pose_data[4], pose_data[5])
    result_matrix = np.dot(zero_matrix, np.dot(np.linalg.inv(begin_matrix), end_matrix))
    xyzrpy = tools.mat2xyzrpy(result_matrix)
    return xyzrpy

This function implements incremental control using the arithmetic rules of transformation matrices. Let's break down the code step by step:

Input Parameters

base_pose: This is the reference pose at the start of teleoperation. When teleoperation is triggered, the system records the pose of the operating device (Pika Sense) at that moment and stores it as self.base_pose. This pose serves as the "starting point" or "reference zero" for calculating all subsequent increments.

pose_data: This is the real-time pose data of the operating device (Pika Sense) received at the current moment.

Matrix Conversion

The function first converts three key poses (expressed in the format [x, y, z, roll, pitch, yaw]) into 4×4 homogeneous transformation matrices. This conversion is typically performed by the tools.xyzrpy2Mat function.

begin_matrix: Converted from base_pose, it represents the pose matrix of the operating device at the start of teleoperation. We denote it as T_begin.

zero_matrix: Converted from self.initial_pose_rpy, it represents the pose matrix of the robotic arm's end effector at the start of teleoperation. This is the "starting point" for the robotic arm's movement. We denote it as T_zero.

end_matrix: Converted from pose_data, it represents the pose matrix of the operating device at the current moment. We denote it as T_end.

Core Calculation

This is the most critical line of code:

result_matrix = np.dot(zero_matrix, np.dot(np.linalg.inv(begin_matrix), end_matrix))

We analyze it using matrix multiplication:

The formula can be expressed as: Result = T_zero × (T_begin)^(-1) × T_end

np.linalg.inv(begin_matrix): Calculates the inverse matrix of begin_matrix, i.e., (T_begin)^(-1). In robotics, the inverse matrix of a transformation matrix represents its reverse transformation.

np.dot(np.linalg.inv(begin_matrix), end_matrix): This step calculates (T_begin)^(-1) × T_end. The physical meaning of this operation is the transformation required to switch from the begin coordinate system to the end coordinate system. In other words, it accurately describes the relative pose change (increment) of the operating device from the start of teleoperation to the current moment. We refer to this increment as ΔT.

np.dot(zero_matrix,...): This step calculates T_zero × ΔT. Its physical meaning is applying the relative pose change (ΔT) just calculated to the initial pose (T_zero) of the robotic arm's end effector.

Result Conversion and Return

xyzrpy = tools.mat2xyzrpy(result_matrix): Converts the calculated 4×4 target pose matrix result_matrix back to the [x, y, z, roll, pitch, yaw] format that the robotic arm controller can understand.

return xyzrpy: Returns the calculated target pose.

Mapping 6D Pose Data to the Robotic Arm

Through incremental control, we obtain the relative pose commands that the robotic arm needs to execute. However, the control commands vary among different robotic arms. This requires writing different control interfaces for each type of robotic arm. For example:

Robotic arms such as Piper and Xarm can directly accept commands in the form of xyzrpy or xyz + quaternion for control. The only difference is that Piper uses the rostopic method for message publishing, while Xarm uses the rosservice method for request sending.

UR robotic arms use the xyz and rotation vector format for command delivery.

In summary, to send the 6D pose data calculated via incremental control to the robotic arm for control, the final step is to adapt to the robotic arm's control interface.

Summary

This article elaborates on the core technical principles of realizing robotic arm teleoperation based on Pika Sense. The entire process can be summarized into four key steps:

Acquire 6D pose data: First, a system composed of Pika Sense and external positioning base stations is used to accurately capture the operator's hand movements. The base stations scan the space using infrared synchronization signals and rotating lasers. The photosensors on Pika Sense receive these signals, real-time solve its high-precision six-degree-of-freedom (6D) pose (position and orientation), and then publish this data via ROS topics or SDK.

Align coordinate systems: Since the coordinate system definitions of Pika Sense and the end effectors of different robotic arms are inconsistent, alignment is essential. By obtaining the respective coordinate system definitions of Pika Sense and the target robotic arm, a transformation matrix is calculated to convert the pose data of Pika Sense into the coordinate system matching the robotic arm's end effector, ensuring the intuitiveness of subsequent control.

Implement incremental control: To enable the robotic arm to smoothly follow the operator's relative movement (rather than jumping abruptly to an absolute position), an incremental control strategy is adopted. This method takes the hand pose and robotic arm pose at the start of teleoperation as references, uses matrix operations to real-time calculate the relative pose change (increment) of the hand from the "starting point" to the "current point", and then applies this increment to the initial pose of the robotic arm to obtain the current target pose of the robotic arm.

Map to the robotic arm: The final step is to send the calculated target pose commands to the robotic arm for execution. Since robotic arms of different brands and models (e.g., Piper, xArm, UR) have distinct control interfaces and communication protocols (e.g., ROS topic, ROS service, specific format commands), corresponding adaptation code needs to be written to format the standard 6D pose data into commands that the specific robotic arm can recognize and execute, ultimately achieving precise teleoperation control.

That's it—four steps to teleoperate any robotic arm with Pika! The magic is in the incremental control: your hand moves 5cm forward, the robot moves 5cm forward. Simple math, smooth motion. We've tested this on Piper, xArm, and UR arms, and the same approach should work for your robot too. Questions? Want to share your teleoperation adventures? Drop a comment below!

Cheers!