环境:ubuntu16.04 ros-kinetic gazebo7.16 turtlebot_teleop包
注意,代码中使用到的中文注释,使用时候一定要删除,否则会报错的.
来源:
以前编写机器人urdf时候,关节控制器编写,控制器yaml编写,把控制器正常加载到ros里面的launch文件编写吃了不少功夫,这次总结一下以往的近一年来摸索的到的经验.下面以一个案例来展示.
案例:
制作一个差速轮式运动机器人,上面有一个顶升以及一个可以绕z轴一定范围内摇动的力臂
案例中测试用到turtlebot_teleop包对机器人手动控制差速测试,记得提前安装.
urdf里面包含了,活动关节类型有:
continuous / prismatic / revolute
;以及使用到的控制器接口类型有, hardware_interface/VelocityJointInterface \ hardware_interface/VelocityJointInterface \ hardware_interface/PositionJointInterface
,
test_joint_controller.urdf.xacro
:
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="test_joint_controller">
<!--base_link-->
<link name="base_link">
<visual>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
<inertial>
<mass value="5" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1" />
</inertial>
</link>
<!--left_wheel-->
<joint name="left_wheel_joint" type="continuous">//关节类型
<parent link="base_link"/>
<child link="left_wheel"/>
<origin xyz="0 0.2 0"/>
<axis xyz="0 1 0" rpy="0 0 0" />//定义转轴是y轴
</joint>
<link name="left_wheel">
<visual>
<geometry>
<cylinder length="0.05" radius="0.1"/>
</geometry>
<origin rpy="1.5707 0 0" xyz="0 0 0"/>
</visual>
<collision>
<geometry>
<cylinder length="0.05" radius="0.1"/>
</geometry>
<origin rpy="1.5707 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="3" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1" />
</inertial>
</link>
<gazebo reference="left_wheel">//定义轮子在gazebo里面物理属性
<mu1 value="200.0"/>
<mu2 value="100.0"/>
<kp value="10000000.0" />
<kd value="1.0" />
<material>Gazebo/Grey</material>
</gazebo>
<!--define transmission which has motor-->
<transmission name="left_wheel_trans">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="left_wheel_motor">//定义接口
<mechanicalReduction>1</mechanicalReduction>//传动比
</actuator>
<joint name="left_wheel_joint">//马达连接的关节
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>//控制器硬件接口类型为速度型
</joint>
</transmission>
<!--right_wheel-->
<joint name="right_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="right_wheel"/>
<origin xyz="0 -0.2 0"/>
<axis xyz="0 1 0" rpy="0 0 0" />
</joint>
<link name="right_wheel">
<visual>
<geometry>
<cylinder length="0.05" radius="0.1"/>
</geometry>
<origin rpy="1.5707 0 0" xyz="0 0 0"/>
</visual>
<collision>
<geometry>
<cylinder length="0.05" radius="0.1"/>
</geometry>
<origin rpy="1.5707 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="3" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1" />
</inertial>
</link>
<gazebo reference="right_wheel">
<mu1 value="200.0"/>
<mu2 value="100.0"/>
<kp value="10000000.0" />
<kd value="1.0" />
<material>Gazebo/Grey</material>
</gazebo>
<!--define transmission which has motor-->
<transmission name="right_wheel_trans">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="right_wheel_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
<joint name="right_wheel_joint">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
</transmission>
<!--idler_wheel-->
<joint name="idler_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="idler_wheel"/>
<origin xyz="0.2 0 0"/>
<axis xyz="0 1 0" rpy="0 0 0" />
</joint>
<link name="idler_wheel">
<visual>
<geometry>
<cylinder length="0.05" radius="0.1"/>
</geometry>
<origin rpy="1.5707 0 0" xyz="0 0 0"/>
</visual>
<collision>
<geometry>
<cylinder length="0.05" radius="0.1"/>
</geometry>
<origin rpy="1.5707 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="3" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1" />
</inertial>
</link>
<gazebo reference="idler_wheel">
<mu1 value="0"/>
<mu2 value="0"/>
<kp value="10000000.0" />
<kd value="1.0" />
<material>Gazebo/Grey</material>
</gazebo>
<!--climbing_mechanism-->
<joint name="climbing_mechanism_to_base_link" type="prismatic">//定义一个prismatic类型的关节,主要是应用定轴移动的关节,案例中采用顶升来展示
<parent link="base_link"/>
<child link="climbing_mechanism"/>
<origin xyz="0 0 0.5"/>
<axis xyz="0 0 1" rpy="0 0 0" />//表示关节移动方向是z方向
<limit effort="100.0" lower="0" upper="0.1" velocity="0.3"/>//这里定义了关节所具有的力大小,移动的范围以及对应移动过程的速度
</joint>
<link name="climbing_mechanism">
<visual>
<geometry>
<cylinder length="0.1" radius="0.03"/>
</geometry>
</visual>
<collision>
<geometry>
<cylinder length="0.1" radius="0.03"/>
</geometry>
</collision>
<inertial>
<mass value="1" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1" />
</inertial>
</link>
<gazebo reference="climbing_mechanism">
<material>Gazebo/Orange</material>
</gazebo>
<!--set a VelocityJointInterface-->
<transmission name="climbing_mechanism_trans">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="climbing_mechanism_motor">
<mechanicalReduction>16</mechanicalReduction>
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>//定义为速度硬件接口,这里一定要记住prismatic关键对应的硬件接口为此
</actuator>
<joint name="climbing_mechanism_to_base_link">//定义硬件接口对应的关节或者表示马达链接的关节
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>//速度类型硬件接口
</joint>
</transmission>
<!--arm_axis-->
<joint name="arm_axis_to_base_link" type="revolute">//定义关节类型为revolute,主要应用是一定幅度范围的部件
<parent link="base_link"/>
<child link="arm_axis"/>
<origin xyz="0.1 0 0.2" rpy="0 0 0"/>
<axis xyz="0 0 1" rpy="0 0 0"/>//定义该关节可以绕z轴转动
<limit lower="-1.57" upper="1.57" effort="1090" velocity="1"/>//定义关节可活动弧度范围,力大小以及转动速度
</joint>
<link name="arm_axis">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
<inertial>
<mass value="0.01"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1"/>
</inertial>
</link>
<!--set a PositionJointInterface-->
<transmission name="arm_axis_trans">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="arm_axis_motor">
<mechanicalReduction>10</mechanicalReduction>
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>//硬件接口类型是位置型
</actuator>
<joint name="arm_axis_to_base_link">//马达链接的关节名称
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>//关节对应的硬件接口是位置类型
</joint>
</transmission>
<!--arm-->
<joint name="arm_to_arm_axis" type="fixed">
<parent link="arm_axis"/>
<child link="arm"/>
<origin xyz="0.3 0 0" rpy="0 1.57 0"/>
</joint>
<link name="arm">
<visual>
<geometry>
<cylinder length="0.6" radius="0.025"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</visual>
<collision>
<geometry>
<cylinder length="0.6" radius="0.025"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="2"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1"/>
</inertial>
</link>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
<legacyModeNS>true</legacyModeNS>
</plugin>
</gazebo>
</robot>
这个yaml文件是对各个活动关节pid进行定义(提供给gazebo的),注意
,yaml文件编写时候一定要按照对应的格式要求,否则是读不进去ros里面,从而导致控制器加载失败,下面是对各个控制器进行定义,test_joint_controller.yaml
:
//滑动类型关节控制器yaml编写样式
climbing_mechanism_controller: //控制器名称自定义
type: "velocity_controllers/JointPositionController"//接口类型一定要对应urdf里面对应的关节定义的
joint: climbing_mechanism_to_base_link//控制器对应的关节
pid: {p: 100.0, i: 0.01, d: 1.0}//这里定义该控制器的pid
//一定范围内转动的关节控制器定义样式
arm_position_controller:
type: "position_controllers/JointGroupPositionController"
joints:
- arm_axis_to_base_link//对应urdf里面定义的关节
//差速运动控制器编写,(这里面也放了一些后面发odom啥的yaml,方便后面做的是建图导航用的,可以忽视的)
mobile_base_controller:
base_frame_id: base_link
enable_odom_tf: false
odom_frame_id: odom
publish_cmd: true
wheel_separation: 0.393
type: diff_drive_controller/DiffDriveController//控制器类型为差素类型
left_wheel: left_wheel_joint//差速的左关节
right_wheel: right_wheel_joint//差素的右关节
publish_rate: 35
pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
//下面是给活动关节提供gazebo仿真所需要的pid(缺少会报错并且机器人可能会在gazebo里面失重,表现异常,如飞天)
gazebo_ros_control:
pid_gains:
# these gains are used by the gazebo_ros_control plugin
climbing_mechanism_to_base_link:
p: 10000
i: 0.00000001
d: 0.00000001
left_wheel_joint:
p: 100
i: 0.001
d: 0.001
right_wheel_joint:
p: 100
i: 0.001
d: 0.001
arm_axis_to_base_link:
p: 500.0
i: 0.01
d: 300.0
这个launch是把urdf以及定义的控制器加载到ros里面,test_joint_controller.launch
:
<?xml version="1.0"?>
<launch>
<!-- Spawn a robot into Gazebo -->
<param name="robot_description" command="$(find xacro)/xacro '$(find rpy)/urdf/test_joint_controller.urdf.xacro' use_nominal_extrinsics:=true --inorder"/>
<node name="urdf_apawner" pkg="gazebo_ros" type="spawn_model" args="-z 1.0 -unparam -urdf -model robot -param robot_description" respawn="false" output="screen"/>
<!--load controller,need to define controller's yaml and define the pid yaml-->
<rosparam command="load" file="$(find rpy)/config/test_joint_controller.yaml"/>
<node name="controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="mobile_base_controller climbing_mechanism_controller arm_position_controller"/>//这里面的控制器名称需要根据yaml定义的一致
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<param name="debug" value="true"/>
<param name="verbose" value="true"/>
</include>
<!--basic node-->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>
<!--node name="rviz" pkg="rviz" type="rviz"/-->
</launch>
这个launch作用是启动键盘的,
test_joint_controller_teleop.launch
:
<?xml version="1.0"?>
<launch>
<!-- turtlebot_teleop_key already has its own built in velocity smoother -->
<node pkg="turtlebot_teleop" type="turtlebot_teleop_key" name="turtlebot_teleop_keyboard" output="screen">
<param name="scale_linear" value="5.0" type="double"/>
<param name="scale_angular" value="1.5" type="double"/>
<remap from="turtlebot_teleop_keyboard/cmd_vel" to="mobile_base_controller/cmd_vel"/>
</node>
</launch>
在终端里面加载机器人,如果有如下信息表示定义的控制器已正常加载到ros里面:
[INFO] [1595420563.704004, 0.500000]: Started controllers: mobile_base_controller, climbing_mechanism_controller, arm_position_controller
下面是把机器人加载到gazebo的图片,方便对比,这里面插入了一个正方体,
然后,这里测试一下climbing_mechanism_controller以及arm_position_controller的效果,分别在新建的终端里面输入
rostopic pub /climbing_mechanism_controller/command std_msgs/Float64 "data: 0.1"
和
rostopic pub /arm_position_controller/command std_msgs/Float64MultiArray "layout:
dim:
- label: ''
size: 0
stride: 0
data_offset: 0
data:
- -1"
机器人效果如图:
差速运动的就不截图了,自己安装一下上面提及的键盘控制包,把那个上面另外一个launch文件在新的终端启动就可以玩了.
个人体会:
控制器正确定义需要urdf里面写好关节以及对应的硬件接口类型,
yaml文件一定要对设置的控制器进行初始化(属于哪个关节的,然后相应的参数有哪些等等),仿真时候一定要在yaml对应活动关节定义好gazebo的对应pid(大小自己测试来写ok了,如果不是很严谨的话),还有yaml文一定一定要注意格式(什么时候空格等等)
个人感觉,搞明白这个案例,基本上轮式移动机器人的urdf以及控制器yaml编写是没问题了.
#######################
好记性不如烂笔头
–20200722