mdh to urdf

urdf文件中要求的是[x, y, z, rx, ry, rz]的表达形式，但是通常机械臂用mdh表示相邻坐标系关系。各大厂商对机械臂标定后通常也是给出dh参数表。

xacro文件支持python的数学运算库，基于此，编写以下转换函数用于[a d alpha theta]向[x, y, z, rx, ry, rz]的转换。

  <xacro:macro name="dh_to_origin" params="a d alpha theta">
    <xacro:property name="r00" value="${cos(theta)}" scope="parent"/>
    <xacro:property name="r10" value="${sin(theta)*cos(alpha)}" scope="parent"/>
    <xacro:property name="r11" value="${cos(theta)*cos(alpha)}" scope="parent"/>
    <xacro:property name="r12" value="${-sin(alpha)}" scope="parent"/>
    <xacro:property name="r20" value="${sin(theta)*sin(alpha)}" scope="parent"/>
    <xacro:property name="r21" value="${cos(theta)*sin(alpha)}" scope="parent"/>
    <xacro:property name="r22" value="${cos(alpha)}" scope="parent"/>
    <xacro:property name="sum" value="${sqrt(r00*r00 + r10*r10)}" scope="parent"/>    


    <xacro:if value="${sum > 1e-15}">
      <xacro:property name="ry" value="0.0" scope="parent"/>
      <xacro:property name="pitch0" value="${atan2(-r20,sum)}" scope="parent"/>
      <xacro:property name="pitch1" value="${atan2(-r20,-sum)}" scope="parent"/>
      <xacro:if value="${fabs(pitch0 - ry) > fabs(pitch1 - ry)}">
        <origin xyz="${a} ${-sin(alpha)*d} ${cos(alpha)*d}" rpy="${atan2(-r21,-r22)} ${pitch1} ${atan2(-r10,-r00)}"/>
      </xacro:if>
      <xacro:unless value="${fabs(pitch0 - ry) > fabs(pitch1 - ry)}">
        <origin xyz="${a} ${-sin(alpha)*d} ${cos(alpha)*d}" rpy="${atan2(r21,r22)} ${pitch0} ${atan2(r10,r00)}"/>
      </xacro:unless>
    </xacro:if>
    <xacro:unless value="${sum > 1e-15}">
      <origin xyz="${a} ${-sin(alpha)*d} ${cos(alpha)*d}" rpy="${atan2(-r12,r11)} ${atan2(-r20,sum)} 0.0"/>
    </xacro:unless>
  </xacro:macro>

使用示例：

    <joint name="${prefix}joint1" type="revolute">      
      <xacro:dh_to_origin  a="${a1}" d="${d1}" alpha="${alpha1}" theta ="${theta1}"/>
      <parent link="${prefix}base_link" />
      <child link="${prefix}link1" />
      <axis xyz="0 0 1" />
      <limit lower="${joint1_limitL}" upper="${joint1_limitU}" effort="${joint1_effort}" velocity="${joint_vel}" />
      <xacro:if value="${safety_limits}">
        <safety_controller soft_lower_limit="${joint1_limitL + safety_pos_margin}" soft_upper_limit="${joint1_limitU - safety_pos_margin}"
          k_position="${safety_k_position}" k_velocity="${safety_k_velocity}"/>
      </xacro:if>
      <dynamics damping="${joint_damp}" friction="${joint_frict}"/>
    </joint>