ROS机器人TF基础(坐标相关概念和实践)

587
0
2020年9月15日 11时30分

TF坐标变换基础

 

机器人建模和控制必须掌握坐标系和坐标变换等基础知识。机器人在空间中运动主要有两种形式:

 

  • 平移和旋转

 

也就是线速度和角速度。

 

ROS机器人TF基础(坐标相关概念和实践)插图

 

坐标关系不仅包括机器人和外部环境,也包括机器人自身各部件,如本体,传感器(摄像头),控制器,执行器(电机)等。

 

ROS机器人TF基础(坐标相关概念和实践)插图(1)

 

TF概念如下图所示:

 

ROS机器人TF基础(坐标相关概念和实践)插图(2)

 

在日常生活中,如果将现实环境建立数学或物理模型必然也离不开坐标系,如下图:

 

ROS机器人TF基础(坐标相关概念和实践)插图(3)

 

常见的如GPS全局坐标,室内定位局部坐标等,在机器人领域示例如下:

 

ROS机器人TF基础(坐标相关概念和实践)插图(4)

 

教程不仅有理论介绍,还有云平台实践环境:

 

ROS机器人TF基础(坐标相关概念和实践)插图(5)

ROS机器人TF基础(坐标相关概念和实践)插图(6)

 

分别展示了两款机器人模型中的TF。

 

下面我们先把坐标系基本概念过一遍:

ROS机器人TF基础(坐标相关概念和实践)插图(7)

ROS机器人TF基础(坐标相关概念和实践)插图(8)

ROS机器人TF基础(坐标相关概念和实践)插图(9)

ROS机器人TF基础(坐标相关概念和实践)插图(10)

ROS机器人TF基础(坐标相关概念和实践)插图(11)

ROS机器人TF基础(坐标相关概念和实践)插图(12)

ROS机器人TF基础(坐标相关概念和实践)插图(13)

ROS机器人TF基础(坐标相关概念和实践)插图(14)

ROS机器人TF基础(坐标相关概念和实践)插图(15)

ROS机器人TF基础(坐标相关概念和实践)插图(16)

 

示例演示:

 

ROS机器人TF基础(坐标相关概念和实践)插图(17)

roslaunch turtle_tf2 turtle_tf2_demo_cpp.launch

 

使用键盘控制控制第一个turtle,第二个跟随,如下图所示:

ROS机器人TF基础(坐标相关概念和实践)插图(18)

                              ROS机器人TF基础(坐标相关概念和实践)插图(19)

 

<launch>
 
  <!-- Turtlesim Node-->
  <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
 
  <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
  <!-- Axes -->
  <param name="scale_linear" value="2" type="double"/>
  <param name="scale_angular" value="2" type="double"/>
 
  <node name="turtle1_tf2_broadcaster" pkg="turtle_tf2" type="turtle_tf2_broadcaster" respawn="false" output="screen" >
    <param name="turtle" type="string" value="turtle1" />
  </node>
  <node name="turtle2_tf2_broadcaster" pkg="turtle_tf2" type="turtle_tf2_broadcaster" respawn="false" output="screen" >
    <param name="turtle" type="string" value="turtle2" />
  </node>
  <node name="turtle_pointer" pkg="turtle_tf2" type="turtle_tf2_listener" respawn="false" output="screen" >
  </node>
 
</launch>

 

注意:由于noetic版本仅支持python3。

 

为了更方便的调试和查看坐标,ROS-TF2提供了大量的工具:

 

ROS机器人TF基础(坐标相关概念和实践)插图(20)

 

import rospy
import tf2_py as tf2
import yaml
import subprocess
from tf2_msgs.srv import FrameGraph
import tf2_ros
 
def main():
    rospy.init_node('view_frames')
    
    # listen to tf for 5 seconds
    rospy.loginfo('Listening to tf data during 5 seconds...')
    rospy.sleep(0.00001)
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)
    rospy.sleep(5.0)
 
    rospy.loginfo('Generating graph in frames.pdf file...')
    rospy.wait_for_service('~tf2_frames')
    srv = rospy.ServiceProxy('~tf2_frames', FrameGraph)
    data = yaml.safe_load(srv().frame_yaml)
    with open('frames.gv', 'w') as f:
        f.write(generate_dot(data))
    subprocess.Popen('dot -Tpdf frames.gv -o frames.pdf'.split(' ')).communicate()
 
def generate_dot(data):
    if len(data) == 0:
        return 'digraph G { "No tf data received" }'
 
    dot = 'digraph G {\n'
    for el in data: 
        map = data[el]
        dot += '"'+map['parent']+'" -> "'+str(el)+'"'
        dot += '[label=" '
        dot += 'Broadcaster: '+map['broadcaster']+'\\n'
        dot += 'Average rate: '+str(map['rate'])+'\\n'
        dot += 'Buffer length: '+str(map['buffer_length'])+'\\n' 
        dot += 'Most recent transform: '+str(map['most_recent_transform'])+'\\n'
        dot += 'Oldest transform: '+str(map['oldest_transform'])+'\\n'
        dot += '"];\n'
        if not map['parent'] in data:
            root = map['parent']
    dot += 'edge [style=invis];\n'
    dot += ' subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";\n'
    dot += '"Recorded at time: '+str(rospy.Time.now().to_sec())+'"[ shape=plaintext ] ;\n'
    dot += '}->"'+root+'";\n}'
    return dot
 
 
if __name__ == '__main__':
    main()

 

ROS机器人TF基础(坐标相关概念和实践)插图(21)

ROS机器人TF基础(坐标相关概念和实践)插图(22)

ROS机器人TF基础(坐标相关概念和实践)插图(23)

ROS机器人TF基础(坐标相关概念和实践)插图(24)

ROS机器人TF基础(坐标相关概念和实践)插图(25)

ROS机器人TF基础(坐标相关概念和实践)插图(26)

ROS机器人TF基础(坐标相关概念和实践)插图(27)

 

坐标为静态关系:

 

ROS机器人TF基础(坐标相关概念和实践)插图(28)

ROS机器人TF基础(坐标相关概念和实践)插图(29)

 

坐标为动态关系:

 

ROS机器人TF基础(坐标相关概念和实践)插图(30)

 

坐标与时间关系:

 

ROS机器人TF基础(坐标相关概念和实践)插图(31)

ROS机器人TF基础(坐标相关概念和实践)插图(32)

 

坐标与机器人关系:

 

ROS机器人TF基础(坐标相关概念和实践)插图(33)

ROS机器人TF基础(坐标相关概念和实践)插图(34)

ROS机器人TF基础(坐标相关概念和实践)插图(35)

ROS机器人TF基础(坐标相关概念和实践)插图(36)

ROS机器人TF基础(坐标相关概念和实践)插图(37)

ROS机器人TF基础(坐标相关概念和实践)插图(38)

 

这个坐标变换TF2工具的意义如上所示,但是可能会觉得不是很清楚,下面举个例子:

 

ROS机器人TF基础(坐标相关概念和实践)插图(39)

 

观察上图绿点的位置,通常我们需要机器人本体坐标,但是传感器并非安装在本体中心,而传感器获取的各类信息都是相对传感器的数据,而非机器人本体,TF2工具可以直接维护这些位置姿态坐标关系,将其转换为机器人本体坐标。

 

ROS机器人TF基础(坐标相关概念和实践)插图(40)

 

tf2常用功能包:

examples-tf2-py: 使用tf2库的Python API示例。

 

geometry2: 用于在ros,tf2中引入默认软件包第二代坐标变换库的元软件包。

 

robot-state-publisher: 状态发布后,对于使用tf2的系统中的所有组件都可用。 该包使用机器人的运动学树模型将机器人的关节角度作为输入,并发布机器人链接的3D姿势。

 

ros-base: 一个扩展“ ros_core”并包含其他基本功能(如tf2和urdf)的软件包。

 

tf2: tf2保持时间缓冲的树结构中的坐标系之间的关系,并允许用户在任意所需的时间点在任意两个坐标系之间转换点,向量等。

 

tf2-ros: 该软件包包含适用于Python和C ++的tf2库的ROS绑定。

 

其他库:tf2-bullet, tf2-eigen, tf2-geometry-msgs, tf2-kdl, tf2-msgs, tf2-py, tf2-sensor-msgs, tf2-tools。

 

机器人模型与TF2工具应用案例:

 

ROS1(noetic,melodic,kinetic,indigo):

 

ROS机器人TF基础(坐标相关概念和实践)插图(41)

 

注意:xacro,sdf等格式适合Gazebo,urdf格式适合rviz。

 

rosrun xacro xacro -o model_out.urdf model_in.urdf.xacro

 

使用如下命令打开一个机器人模型(此处不要求掌握,后续详细介绍):

 

roslaunch robot1_description display.launch model:=robot1_base1.urdf

 

ROS机器人TF基础(坐标相关概念和实践)插图(42)

 

rqt-tf-tree:

 

ROS机器人TF基础(坐标相关概念和实践)插图(43)

 

rosrun tf2_tools echo.py base_link wheel_1

 

ROS机器人TF基础(坐标相关概念和实践)插图(44)

 

rosrun tf2_tools view_frames.py

 

ROS机器人TF基础(坐标相关概念和实践)插图(45)ROS机器人TF基础(坐标相关概念和实践)插图(46)

 

ROS2(foxy,dashing):

机器人urdf模型通用!!!

 

ros2 launch urdf_tutorial display.launch.py model:=src/urdf/robot1_base1.urdf

 

ROS机器人TF基础(坐标相关概念和实践)插图(47)

 

下图和ros1几乎一样:

 

ROS机器人TF基础(坐标相关概念和实践)插图(48)

 

同样也能使用各类工具:

 

ros2 run tf2_tools view_frames.py

 

ROS机器人TF基础(坐标相关概念和实践)插图(49)

ROS机器人TF基础(坐标相关概念和实践)插图(50)

ROS机器人TF基础(坐标相关概念和实践)插图(51)

 

ROS1和ROS2几乎一致!

 

下一节,将通过URDF构建机器人模型,以ROS2(foxy)讲解为主!主要是ROS1相关URDF功能如何顺利移植到ROS2中!

 

附录(ros2 foxy):

 

display.launch.py

import launch
from launch.substitutions import Command, LaunchConfiguration
import launch_ros
import os
 
 
def generate_launch_description():
    pkg_share = launch_ros.substitutions.FindPackageShare(package='urdf_tutorial').find('urdf_tutorial')
    default_model_path = os.path.join(pkg_share, 'urdf/01-myfirst.urdf')
    default_rviz_config_path = os.path.join(pkg_share, 'rviz/urdf.rviz')
 
    robot_state_publisher_node = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}]
    )
    joint_state_publisher_node = launch_ros.actions.Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher',
        condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui'))
    )
    joint_state_publisher_gui_node = launch_ros.actions.Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
        name='joint_state_publisher_gui',
        condition=launch.conditions.IfCondition(LaunchConfiguration('gui'))
    )
    rviz_node = launch_ros.actions.Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', LaunchConfiguration('rvizconfig')],
    )
 
    return launch.LaunchDescription([
        launch.actions.DeclareLaunchArgument(name='gui', default_value='True',
                                             description='Flag to enable joint_state_publisher_gui'),
        launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path,
                                             description='Absolute path to robot urdf file'),
        launch.actions.DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path,
                                             description='Absolute path to rviz config file'),
        joint_state_publisher_node,
        joint_state_publisher_gui_node,
        robot_state_publisher_node,
        rviz_node
    ])

 

robot1_base1.urdf

 

<?xml version="1.0" ?>
 
<robot name="robot1_xacro" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor">
  <link name="base_footprint">
    <visual>
      <geometry>
        <box size="0.001 0.001 0.001"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <inertial>
      <mass value="0.0001"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <gazebo reference="base_footprint">
    <material>Gazebo/Green</material>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <joint name="base_footprint_joint" type="fixed">
    <origin xyz="0 0 0"/>
    <parent link="base_footprint"/>
    <child link="base_link"/>
  </joint>
  <link name="base_link">
    <visual>
      <geometry>
        <box size="0.2 .3 .1"/>
      </geometry>
      <origin rpy="0 0 1.54" xyz="0 0 0.05"/>
      <material name="white">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.2 .3 0.1"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="10"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <link name="wheel_1">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.05"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="black">
        <color rgba="0 0 0 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.05"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <link name="wheel_2">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.05"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="black"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.05"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <link name="wheel_3">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.05"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="black"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.05"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <link name="wheel_4">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.05"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="black"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.05"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <joint name="base_to_wheel1" type="continuous">
    <parent link="base_link"/>
    <child link="wheel_1"/>
    <origin rpy="1.5707 0 0" xyz="0.1 0.15 0"/>
    <axis xyz="0 0 1"/>
  </joint>
  <joint name="base_to_wheel2" type="continuous">
    <axis xyz="0 0 1"/>
    <anchor xyz="0 0 0"/>
    <limit effort="100" velocity="100"/>
    <parent link="base_link"/>
    <child link="wheel_2"/>
    <origin rpy="1.5707 0 0" xyz="-0.1 0.15 0"/>
  </joint>
  <joint name="base_to_wheel3" type="continuous">
    <parent link="base_link"/>
    <axis xyz="0 0 1"/>
    <child link="wheel_3"/>
    <origin rpy="1.5707 0 0" xyz="0.1 -0.15 0"/>
  </joint>
  <joint name="base_to_wheel4" type="continuous">
    <parent link="base_link"/>
    <axis xyz="0 0 1"/>
    <child link="wheel_4"/>
    <origin rpy="1.5707 0 0" xyz="-0.1 -0.15 0"/>
  </joint>
</robot>

 

~Fin~

发表评论

后才能评论