urdf 文件 joint origin标签 rpy变换理解！

1. 示例代码

https://download.csdn.net/download/qq_42226250/13545188

demo.urdf：

<?xml version="1.0" encoding="UTF-8" ?>
<robot name="joint_rpy_demo">
    <link name="base_footprint">
    </link>
    <link name="base_link">
    </link>
    <joint name="base_joint" type="fixed">
        <origin xyz="0 0 1.0" rpy="0 0 0" />
        <parent link="base_footprint"/>
        <child link="base_link" />
    </joint>
</robot>

launch文件：

<?xml version="1.0" encoding="UTF-8" ?> 
<launch>  
    <param name="robot_description" textfile="$(find pkg)/urdf/demo.urdf" /> 
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/> 
    <node pkg="rviz" type="rviz" name="rviz" /> 
</launch>

roslaunch demo_joint_rpy demo_joint_rpy.launch

初始rviz示意图：base_link位于base_footprint上方1米位置，<origin xyz="0 0 1.0" rpy="0 0 0" />。

2. joint rpy基础知识

<origin xyz="0 0 1.0" rpy="1.57 0 0" />
<parent link="base_footprint"/>
<child link="base_link" />

rpy都是弧度制，PI = 3.14 rad = 180°。

所有变换均基于parent坐标系：base_footprint，这个很重要，parent坐标系是不变的，变的是child坐标系。

rpy：roll、pitch、yaw，翻滚、俯仰、偏航，对应x、y、z顺序。从坐标原点为基准，看向坐标轴正方向，绕坐标轴顺时针为正，逆时针为负。

3. joint rpy测试–一次变换

<origin xyz="0 0 1.0" rpy="1.57 0 0" />

绕x轴顺时针90°：

<origin xyz="0 0 1.0" rpy="0 1.57 0" />

绕y轴顺时针90°：

<origin xyz="0 0 1.0" rpy="0 0 1.57" />

绕z轴顺时针90°：

4. joint rpy测试–多次变换

<origin xyz="0 0 1.0" rpy="1.57 1.57 0" />

先绕x轴顺时针90°，再绕y轴顺时针90°：

注意这里有一个先后顺序！！！
一定是先roll，再pitch，最后yaw！！！

<origin xyz="0 0 1.0" rpy="1.57 1.57 -1.57" />

先绕x轴顺时针90°，再绕y轴顺时针90°，最后绕z轴逆时针90°：

注意：这些绕的xyz轴都是parent坐标系的坐标轴！！！

5. 逆解RPY（重点！！！)

很多情况是我们先知道了child坐标系的位姿，再去求转换的rpy的。
例如下图：如何求得rpy？<origin xyz="0 0 1.0" rpy="？ ？ ？" />

逆解RPY时，基准坐标系还是base_footprint！！

方法一：
想象一个base_link2和base_footprint同位姿，按照RPY顺序旋转，顺正逆负。

so：

<origin xyz="0 0 1.0" rpy="+/-3.14 1.57 0" />

方法二：
由base_link坐标，按照YPR顺序，逆向推出，基准坐标系同样是base_footprint，顺正逆负，但最后结果要全部取反。

so:

<origin xyz="0 0 1.0" rpy="0 1.57 -3.14" />

当然还有其他变换也会产生这样的位姿：

<origin xyz="0 0 1.0" rpy="-1.57 1.57 1.57" />

掌握转换方法即可。