稳定的旋转矩阵到四元数转换

主运行循环

Configuration methods

Set steering angles for all wheels.

Set steering angles for all wheels. angles: dictionary with keys 'fl', 'fr', 'rl', 'rr' and desired angles in radians

Set target velocities for all wheels.

使用指数移动平均和移动平均结合的方法平滑位置

平滑旋转四元数 - 改进版本

启动键盘监听

Stop the robotic arm; this function must be called first when exiting the teaching mode for the first time to control the robotic arm in CAN mode

Stop the robotic arm; this function must be called first when exiting the teaching mode for the first time to control the robotic arm in CAN mode

停止机械臂；初次退出示教模式需先调用此函数才能使用CAN模式控制机械臂

停止机械臂；初次退出示教模式需先调用此函数才能使用CAN模式控制机械臂

停止机械臂；初次退出示教模式需先调用此函数才能使用CAN模式控制机械臂

Stop robotic arm; must call this function when first exiting teach mode before using CAN mode

Stop robotic arm; must call this function when first exiting teach mode before using CAN mode

停止机械臂；初次退出示教模式需先调用此函数才能使用CAN模式控制机械臂

停止

分段三次样条拟合（保证C2连续性）

加权四元数平均，更重视最近的数据

Constructors and mutators

曲线交叉调整

分析曲线端点

Apply joint offsets to solution

计算曲线长度

Calculate distance between two joint configurations

Calculate wrist center position

计算手掌边界框的中心点

从camera_info消息中获取相机内参

曲线交叉检测

ROS cmd_vel回调函数

Callback for cmd_vel topic.

Compute analytical Jacobian (more efficient but convention-specific)

Main analytical IK solver

Main IK solver with verification

IK computation with continuity enforcement

Compute Jacobian matrix numerically

Compute pose error (position and orientation)

Compute rotation matrix from base to joint 3