Analytical Single-step IK for EE control of SO101

<aside> 🤗

觉得有用的话，欢迎给我的XLeRobot Github Repo一个star！

XLeRobot/simulation/Maniskill/codes at main · Vector-Wangel/XLeRobot

</aside>

<aside> 💭

English Version

The controller decouples traditional IK into different components that can be either directly controlled or solved via simple analytical solutions (law of cosines).

Horizontal Plane Direction/Angle

• Polar coordinates instead of Cartesian coordinates, allowing direct control by 1st joint
• Advantages of polar coordinates over Cartesian coordinates:
  • More intuitive for human, simplifying keyboard control and VR control difficulty
  • Beneficial for task planning due to rotational symmetry - can reduce search space and state space from 3D to 2D in many cases
  • Enables symmetric policy training, only needing to focus on the vertical plane.
    • Building training datasets in polar coordinate can greatly improve VLA model training efficiency and policy generalization ability
    • E.g. Dian Wang's series of work during his PhD on using Equivariant Models to improve RL, VLA (Equivariant Diffusion Policy) to improve the efficiency and generalization of robot learning

Vertical Plane 2D Position

• Without considering robot orientation control, the vertical plane becomes a 2-joint linkage controlled by 2nd and 3rd joint, which has a unique single analytical solution under SO100's angle>0 constraint. Can be solved in one step using law of cosines

Tip Orientation

• Since it's 5DOF, yaw cannot be changed
  • (We can also utilize this algorithm for 6dof arms of similar structure)
• SO100's 4th joint directly corresponds to pitch
  • when calculating pitch, need to compensate for the height in previous vertical plane , but still has a unique analytical solution
• SO100's 5th joint directly corresponds to roll </aside>

<aside> 💭

Chinese Blog:

控制器将传统逆运动学解耦为不同的组件，这些组件可以直接控制或通过简单的解析解决方案（余弦定理）求解。

水平面方向/角度

• 极坐标替代笛卡尔坐标，通过第1关节实现直接控制
• 极坐标相对于笛卡尔坐标的优势：
  • 对人类来说更直观，简化键盘控制和VR控制难度
  • 有利于任务规划，因为旋转对称性可以在许多情况下将搜索空间和状态空间从3D降至2D
  • 实现对称策略训练，只需专注于垂直平面。
    • 在极坐标系中构建训练数据集可以大大提高VLA模型训练效率和策略泛化能力
    • 例如：Dian Wang在其博士期间关于使用等变模型改进RL、VLA的一系列工作（Equivariant Diffusion Policy），提高机器人学习的效率和泛化能力

垂直平面2D位置

• 不考虑机器人方向控制时，垂直平面变成由第2和第3关节控制的双关节连杆，在SO100的角度>0约束下具有唯一的解析解。可以使用余弦定理一步求解

末端朝向

• 由于是5自由度，偏航无法改变
  • （我们也可以将此算法用于类似结构的6自由度机械臂）
• SO100的第4关节直接对应俯仰角
  • 计算俯仰角时需要补偿前一个垂直平面的高度，但仍然有唯一的解析解
• SO100的第5关节直接对应横滚角 </aside>

代码实现 Show Me the Code

def inverse_kinematics(x, y, l1=0.1159, l2=0.1350):
    """
    这里展示最主要的垂直2D平面的2-link的IK，余弦定理计算，其他角度都是一对一独立控制
    """
    # 基于urdf算角度offset
    theta1_offset = -math.atan2(0.028, 0.11257)  
    theta2_offset = -math.atan2(0.0052, 0.1349) + theta1_offset  

    # 目标点到原点距离，如果超过范围则缩放到最近点
    r = math.sqrt(x**2 + y**2)
		r_max, r_min = l1 + l2, abs(l1 - l2)
		scale = (l1 + l2)/r if r > (l1 + l2) else ((l2 - l1)/r if 0 < r < (l2 - l1) else 1)
		x, y, r = x*scale, y*scale, r*scale
    
    # 余弦定理与角度计算
    cos_theta2 = -(r**2 - l1**2 - l2**2) / (2 * l1 * l2)
    theta2 = math.pi - math.acos(cos_theta2)
    theta1 = math.atan2(y, x) + math.atan2(l2 * math.sin(theta2), l1 + l2 * math.cos(theta2))
    
    # 加上offsets并保证在角度限制
    joint2 = max(-0.1, min(3.45, theta1 - theta1_offset))
    joint3 = max(-0.2, min(math.pi, theta2 - theta2_offset))
    
    return joint2, joint3

代码完整版（以及键盘尖端控制与VR尖端控制代码）见 Complete codes：

https://github.com/Vector-Wangel/XLeRobot/blob/main/simulation/Maniskill/codes/demo_ctrl_action_ee.py

仿真验证与实验

EE Control of SO101 based on this IK function

理论验证/Validation

1747141373069.mov

VR control

hand tracking with Quest3

1747376552464.mov

基于键盘EE控制实现的一系列家庭任务

A series of household tasks implemented using keyboard-based end-effector control

32474.mov