一、具体步骤
1. Solidworks导出urdf模型
参考博客:我说的不清楚地方可以看看他的,从solidworks导出URDF模型
1.1 sw_urdf_exporter插件
下载sw_urdf_exporter插件,地址,下载安装即可,安装完成后就能在solidworks中导出URDF模型了。
1.2 solidworks启动!
我绘制了一个舵机平台,两个自由度。
1.3 定义link和joint
麻烦的是定义link和joint。定义时需要思路清晰,每一个运动的刚体块只需用定义一个坐标系就行。比如我的模型中,下面的舵机定义一个base_link,上面的舵机定义一个link2,上面舵机转轴处定义一个link,然后在每个舵机齿轮处定义转轴joint,也就是那个圆柱的基准轴。
请注意每一个child_link的原点都需要与上一个joint转轴重合。
1.4 打开插件进行配置
我用的2023版本的sw,在这个地方,“工具—Tool—Export as URDF”
下面是该插件的界面,红框1是本配置界面的link名字,红框2是本link参考的坐标系(上面定义的),红框3是本link关联的刚体,红框4是本link的子坐标系,红框5是预览最终配置,红框6是整个配置过程的link树。
这个配置过程是一步步进行的,定义完第一个link之后,点击link树里面的link再配置下一个。
点击第二个link之后,就会多出joint的配置,红框1是link和joint的名字,随便取(最好保证可读性),红框2是参考的坐标系和转轴(之前定义的坐标系和转轴),红框3是joint的运动方式、该link关联的刚体和该link的子系坐标系(可以很多个,我这只有一个)。注意这里关联刚体的选择不能选到下一个link去了,也就是说所有的刚体总和是整个装配体,不能重复关联刚体。
定义完成之后就点一下Preview and Export,没啥问题就点一下NEXT,然后生成urdf和meshes,这个meshes里面是stl文件。
这里导出也有个坑,必须把文件名改成纯英文,不能有 “.”、中文、不能大写开头。因为这个名字它会用到里面功能包、find_pkg等等地方(踩过坑^-^)
2.使用导出的urdf包到gazebo中
参考博客:我说的不清楚地方可以看看他的,Gazebo仿真小例程一(通过例程熟悉整个仿真步骤)
2.1 试运行
上一步导出的模型文件,包括下面的文件,这是一个ros功能包,需要再ubuntu
里面新建工作空间,然后编译,然后运行gazebo.launch
。
这个时候应该运行出了一个简单的模型,这个时候他不能用ros控制joint,只能看看模型对不对^-^。
2.2 加入电机控制
2.2.1 安装依赖
先安装ros_control
sudo apt-get install ros-$ROS_DISTRO-ros-control ros-$ROS_DISTRO-ros-controllers
一个电机控制器的硬件接口
sudo apt-get install ros-$ROS_DISTRO-effort-controllers
下面安装它的电机及ros控制接口。主要修改三个文件:urdf、yaml、launch
2.2.2 urdf
2.2.1.1 修改后缀名
***.urdf -> ***.urdf.xacro
然后在原有基础上加入其他标签,这些文件都最好不要有中文注释,所以我就不把注释写进去了。
2.2.1.2 dynamic标签
这个标签在link里面加上,加在link最后面就行,第一个属性damping是阻尼系数,我们设置为0.7,第二个标签是摩擦系数,我们设置为0.5。
<dynamic damping="0.7" friction="0.5"/>
2.2.1.3 transmission标签
这个标签我们加在robot根标签最后面。这个标签主要是设置关节驱动器,关节驱动器一般是电机,舵机这些,用于驱动关节,改变关节变量用的,每个活动关节都需要设置一个关节驱动电机,设置的每个关节驱动电机都需要和对应的关节绑定,以此来说明改驱动器驱动该关节。
注意:
joint name需要与关节名称对应
<mechanicalReduction>1</mechanicalReduction>:关节制动器之间机械减速比
<!-- motor -->
<transmission name="joint_1_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint1">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="joint_1_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="joint_2_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint2">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="joint_2_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
2.2.1.4 ros_control插件
此外我们还需要用gazebo标签来载入ros_control插件,由于我们整个驱动控制是基于ros_control做的,因此需要载入该插件,这个插件会给我们自动处理其余的任务,我们无需自己配置。
需要注意的是下面的robotNamespace标签,这个标签声明命名空间,这个需要留意一下。
<!-- ros_control -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/two_degree_platform</robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
<legacyModeNS>true</legacyModeNS>
</plugin>
</gazebo>
修改完我的urdf如下所示:
<?xml version="1.0" encoding="utf-8"?>
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
Commit Version: 1.6.0-4-g7f85cfe Build Version: 1.6.7995.38578
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot
name="two_degree_platform">
<link
name="base_link">
<inertial>
<origin
xyz="0.00359638015135648 -0.000379875233127713 0.0145072443773626"
rpy="0 0 0" />
<mass
value="5.0" />
<inertia
ixx="4.91479288608741E-05"
ixy="-9.94504456334803E-13"
ixz="-1.18860548321916E-06"
iyy="2.15613270914037E-05"
iyz="-1.69011842238603E-07"
izz="4.81596009383514E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://two_degree_platform/meshes/base_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.2 0.2 0.2 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://two_degree_platform/meshes/base_link.STL" />
</geometry>
</collision>
<dynamic damping="0.7" friction="0.5"/>
</link>
<link
name="link1">
<inertial>
<origin
xyz="-0.000451886556456518 0.0038421620877799 0.0217267806896146"
rpy="0 0 0" />
<mass
value="0.0267515083097359" />
<inertia
ixx="4.65491008108007E-06"
ixy="4.32541508067955E-11"
ixz="5.82253120679287E-08"
iyy="4.05857918132744E-06"
iyz="2.7475382716064E-08"
izz="2.34864008475162E-06" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://two_degree_platform/meshes/link1.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://two_degree_platform/meshes/link1.STL" />
</geometry>
</collision>
<dynamic damping="0.7" friction="0.5"/>
</link>
<joint
name="joint1"
type="continuous">
<origin
xyz="0.0018683 0.0093 0.0615"
rpy="0 0 -0.11271" />
<parent
link="base_link" />
<child
link="link1" />
<axis
xyz="0 0 1" />
</joint>
<link
name="link2">
<inertial>
<origin
xyz="1.06897823926033E-12 -0.0274897014708804 0.0134940267476799"
rpy="0 0 0" />
<mass
value="0.00545761201132931" />
<inertia
ixx="2.65135727351373E-06"
ixy="6.48509600241574E-22"
ixz="-8.20563167652603E-22"
iyy="6.96938113125332E-07"
iyz="-3.42783645841975E-21"
izz="2.30728628291268E-06" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://two_degree_platform/meshes/link2.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://two_degree_platform/meshes/link2.STL" />
</geometry>
</collision>
<dynamic damping="0.7" friction="0.5"/>
</link>
<joint
name="joint2"
type="continuous">
<origin
xyz="-0.00079267 0.029192 0.03487"
rpy="0 -0.1988 0" />
<parent
link="link1" />
<child
link="link2" />
<axis
xyz="0 -1 0" />
</joint>
<link
name="plat_link">
<inertial>
<origin
xyz="1.02695629777827E-14 1.01363362148277E-12 -0.0149291851817156"
rpy="0 0 0" />
<mass
value="1.0" />
<inertia
ixx="0.155943555803937"
ixy="-2.42861286636753E-17"
ixz="8.02309607639273E-18"
iyy="0.155943555803937"
iyz="6.93889390390723E-18"
izz="0.310773464453391" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://two_degree_platform/meshes/plat_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://two_degree_platform/meshes/plat_link.STL" />
</geometry>
</collision>
<dynamic damping="0.7" friction="0.5"/>
</link>
<joint
name="joint3"
type="fixed">
<origin
xyz="0 -0.0278 0.057"
rpy="0 0 -1.5708" />
<parent
link="link2" />
<child
link="plat_link" />
<axis
xyz="0 0 0" />
</joint>
<!-- color -->
<gazebo reference="base_link">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="link1">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="link2">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="plat_link">
<material>Gazebo/White</material>
</gazebo>
<!-- motor -->
<transmission name="joint_1_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint1">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="joint_1_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="joint_2_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint2">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="joint_2_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<!-- ros_control -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/two_degree_platform</robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
<legacyModeNS>true</legacyModeNS>
</plugin>
</gazebo>
</robot>
2.2.3 yaml
他自动生成的yaml没啥用,直接替换内容!
这里定义电机控制的参数和策略,需要填上我们的关节名称,以及pid参数,这里的PID参数大小需要根据你的模型重量等等参数来改,如果很不匹配,模拟出来的模型就会一坨^-^(体验过),记下这个控制器的名字,launch里面会用。
two_degree_platform:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
# Position Controllers ---------------------------------------
joint1_position_controller:
type: effort_controllers/JointPositionController
joint: joint1
pid: {p: 1.0, i: 0.01, d: 0.0}
joint2_position_controller:
type: effort_controllers/JointPositionController
joint: joint2
pid: {p: 1.0, i: 0.0, d: 0.0}
2.2.4 launch
直接贴出我的launch,修改重点除了改改文件路径及名称,就是后面的controller_manager功能包的arg参数,改成yaml的电机控制器的名称。
<launch>
<arg name="model" default="$(find two_degree_platform)/urdf/two_degree_platform.urdf.xarco"/>
<param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
<!-- these are the arguments you can pass this launch file, for example paused:=true -->
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find gazebo_ros)/worlds/gazebo_demo.world"/>
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model myrobot -param robot_description"/>
<rosparam file="$(find two_degree_platform)/config/joint_names_two_degree_platform.yaml" command="load"/>
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/two_degree_platform" args="joint_state_controller joint1_position_controller joint2_position_controller"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
respawn="false" output="screen">
<remap from="/joint_states" to="/two_degree_platform/joint_states" />
</node>
</launch>
2.3 运行及话题控制
进入工作空间编译然后source
cd your_ws && catkin_make
source ./devel.setup.bash
roslaunch your_pkg your_launch.launch
这里报了个黄,没啥事,是说惯性参数啥的受到影响啥的。
然后出来了!
康康话题,这两个是控制话题
发送角度控制指令,平台动了:
3. 加入到小车底盘上
核心就是在原有机器人的urdf上加上上面的link和joint,,再把yaml加进去,然后把launch整合一下就可以了。
urdf.xacro:
<?xml version="1.0" encoding="utf-8"?>
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot
name="hunter2_base">
<link
name="base_link">
<inertial>
<origin
xyz="0.0266989327622036 0.000409203500587652 -0.135901124750884"
rpy="0 0 0" />
<mass
value="21.02945169536679" />
<inertia
ixx="0.0332370959804736"
ixy="-1.013150489971E-06"
ixz="-0.00188859201421112"
iyy="0.111072002332437"
iyz="-5.13308150598312E-07"
izz="0.12660862809283" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter2_base/meshes/base_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter2_base/meshes/base_link.STL" />
</geometry>
</collision>
</link>
<link
name="front_steer_left_link">
<inertial>
<origin
xyz="-0.012614 1.8116E-05 0.0023132"
rpy="0 0 0" />
<mass
value="7.8526" />
<inertia
ixx="0.063827"
ixy="0"
ixz="0"
iyy="0.063827"
iyz="0"
izz="0.11091" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder length="0.001" radius="0.005"/>
</geometry>
<material
name="">
<color
rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder length="0.001" radius="0.005"/>
</geometry>
</collision>
</link>
<joint
name="front_steer_left_joint"
type="revolute">
<origin
xyz="0.37142 0.29199 -0.1955"
rpy="1.5708 0 0" />
<parent
link="base_link" />
<child
link="front_steer_left_link" />
<axis
xyz="0 1 0" />
<limit
lower="-0.69"
upper="0.69"
effort="0"
velocity="0" />
</joint>
<link
name="front_left_wheel_link">
<inertial>
<origin
xyz="-6.5774E-06 2.0242E-05 -0.0096386"
rpy="0 0 0" />
<mass
value="7.7007" />
<inertia
ixx="0.063724"
ixy="0"
ixz="0"
iyy="0.063724"
iyz="0"
izz="0.11072" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter2_base/meshes/front_left_wheel_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter2_base/meshes/front_left_wheel_link.STL" />
</geometry>
</collision>
</link>
<joint
name="front_left_wheel_joint"
type="continuous">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="front_steer_left_link" />
<child
link="front_left_wheel_link" />
<axis
xyz="0 0 -1" />
<dynamics damping="0.0" friction="15"/>
</joint>
<link
name="front_steer_right_link">
<inertial>
<origin
xyz="-8.9374E-08 1.0997E-07 0.0095958"
rpy="0 0 0" />
<mass
value="7.696" />
<inertia
ixx="0.063704"
ixy="0"
ixz="0"
iyy="0.063704"
iyz="0"
izz="0.11068" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder length="0.001" radius="0.005"/>
</geometry>
<material
name="">
<color
rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder length="0.001" radius="0.005"/>
</geometry>
</collision>
</link>
<joint
name="front_steer_right_joint"
type="revolute">
<origin
xyz="0.37142 -0.29201 -0.1955"
rpy="1.5708 0 0" />
<parent
link="base_link" />
<child
link="front_steer_right_link" />
<axis
xyz="0 1 0" />
<limit
lower="-0.69"
upper="0.69"
effort="0"
velocity="0" />
</joint>
<link
name="front_right_wheel_link">
<inertial>
<origin
xyz="-9.1148E-08 1.1284E-07 0.0095049"
rpy="0 0 0" />
<mass
value="7.5613" />
<inertia
ixx="0.063626"
ixy="0"
ixz="0"
iyy="0.063626"
iyz="0"
izz="0.11054" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 3.14 0" />
<geometry>
<mesh
filename="package://hunter2_base/meshes/front_left_wheel_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter2_base/meshes/front_right_wheel_link.STL" />
</geometry>
</collision>
</link>
<joint
name="front_right_wheel_joint"
type="continuous">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="front_steer_right_link" />
<child
link="front_right_wheel_link" />
<axis
xyz="0 0 -1" />
<dynamics damping="0.0" friction="15"/>
</joint>
<link
name="left_rear_link">
<inertial>
<origin
xyz="-8.826E-05 6.4342E-05 -0.0096491"
rpy="0 0 0" />
<mass
value="7.6692" />
<inertia
ixx="0.063699"
ixy="0"
ixz="0"
iyy="0.063699"
iyz="0"
izz="0.11067" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter2_base/meshes/left_rear_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter2_base/meshes/left_rear_link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_rear_joint"
type="continuous">
<origin
xyz="-0.28 0.29249 -0.19558"
rpy="1.5708 0 0" />
<parent
link="base_link" />
<child
link="left_rear_link" />
<axis
xyz="0 0 -1" />
<dynamics damping="0.0" friction="15"/>
</joint>
<link
name="right_rear_link">
<inertial>
<origin
xyz="8.6344E-05 7.4488E-05 0.0097824"
rpy="0 0 0" />
<mass
value="7.7133" />
<inertia
ixx="0.06373"
ixy="0"
ixz="0"
iyy="0.06373"
iyz="0"
izz="0.11073" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter2_base/meshes/right_rear_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://hunter2_base/meshes/right_rear_link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_rear_joint"
type="continuous">
<origin
xyz="-0.28018 -0.29251 -0.19558"
rpy="1.5708 0.0026575 0" />
<parent
link="base_link" />
<child
link="right_rear_link" />
<axis
xyz="0 0 -1" />
<dynamics damping="0.0" friction="15"/>
</joint>
<link
name="front_steer_link">
<inertial>
<origin
xyz="0.049836 8.1046E-15 0.017912"
rpy="0 0 0" />
<mass
value="0.0049179" />
<inertia
ixx="1.6846E-07"
ixy="7.6396E-23"
ixz="-1.0974E-08"
iyy="7.549E-07"
iyz="-1.8957E-22"
izz="7.334E-07" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder length="0.001" radius="0.005"/>
</geometry>
<material
name="">
<color
rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder length="0.001" radius="0.005"/>
</geometry>
</collision>
</link>
<joint
name="front_steer_joint"
type="revolute">
<origin
xyz="0.44792 -1.1431E-05 -0.121"
rpy="0 0 0" />
<parent
link="base_link" />
<child
link="front_steer_link" />
<axis
xyz="0 0 1" />
<limit
lower="-0.72"
upper="0.72"
effort="0"
velocity="0" />
</joint>
<link
name="rear_wheel_link">
<inertial>
<origin
xyz="-0.02 0.0031269 -0.12735"
rpy="0 0 0" />
<mass
value="0.0049179" />
<inertia
ixx="1.1821E-06"
ixy="9.6965E-13"
ixz="-3.1553E-12"
iyy="9.5276E-07"
iyz="1.2428E-08"
izz="1.0913E-06" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder length="0.001" radius="0.005"/>
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<cylinder length="0.001" radius="0.005"/>
</geometry>
</collision>
</link>
<joint
name="rear_wheel_joint"
type="continuous">
<origin
xyz="-0.42408 -1.1431E-05 -0.085581"
rpy="0.0032313 0 0" />
<parent
link="base_link" />
<child
link="rear_wheel_link" />
<axis
xyz="0 0 1" />
<dynamics damping="0.0" friction="15"/>
</joint>
<!-- platform -->
<joint name="steering_gear_1_joint" type="fixed">
<parent link="base_link"/>
<child link="steering_gear_1_link"/>
<origin xyz="0.0 0.0 0.05" rpy="0 0 0"/>
</joint>
<link
name="steering_gear_1_link">
<inertial>
<origin
xyz="0.00359638015135648 -0.000379875233127713 0.0145072443773626"
rpy="0 0 0" />
<mass
value="0.001" />
<inertia
ixx="4.91479288608741E-05"
ixy="-9.94504456334803E-13"
ixz="-1.18860548321916E-06"
iyy="2.15613270914037E-05"
iyz="-1.69011842238603E-07"
izz="4.81596009383514E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://two_degree_platform/meshes/base_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.2 0.2 0.2 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://two_degree_platform/meshes/base_link.STL" />
</geometry>
</collision>
<dynamic damping="0.7" friction="0.5"/>
</link>
<link
name="steering_gear_2_link">
<inertial>
<origin
xyz="-0.000451886556456518 0.0038421620877799 0.0217267806896146"
rpy="0 0 0" />
<mass
value="0.001" />
<inertia
ixx="4.65491008108007E-06"
ixy="4.32541508067955E-11"
ixz="5.82253120679287E-08"
iyy="4.05857918132744E-06"
iyz="2.7475382716064E-08"
izz="2.34864008475162E-06" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://two_degree_platform/meshes/link1.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://two_degree_platform/meshes/link1.STL" />
</geometry>
</collision>
<dynamic damping="0.7" friction="0.5"/>
</link>
<joint
name="steering_gear_2_joint"
type="continuous">
<origin
xyz="0.0018683 0.0093 0.0615"
rpy="0 0 -0.11271" />
<parent
link="steering_gear_1_link" />
<child
link="steering_gear_2_link" />
<axis
xyz="0 0 1" />
</joint>
<link
name="platform_1_link">
<inertial>
<origin
xyz="1.06897823926033E-12 -0.0274897014708804 0.0134940267476799"
rpy="0 0 0" />
<mass
value="0.001" />
<inertia
ixx="2.65135727351373E-06"
ixy="6.48509600241574E-22"
ixz="-8.20563167652603E-22"
iyy="6.96938113125332E-07"
iyz="-3.42783645841975E-21"
izz="2.30728628291268E-06" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://two_degree_platform/meshes/link2.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://two_degree_platform/meshes/link2.STL" />
</geometry>
</collision>
<dynamic damping="0.7" friction="0.5"/>
</link>
<joint
name="platform_1_joint"
type="continuous">
<origin
xyz="-0.00079267 0.029192 0.03487"
rpy="0 -0.1988 0" />
<parent
link="steering_gear_2_link" />
<child
link="platform_1_link" />
<axis
xyz="0 -1 0" />
</joint>
<link
name="platform_2_link">
<inertial>
<origin
xyz="1.02695629777827E-14 1.01363362148277E-12 -0.0149291851817156"
rpy="0 0 0" />
<mass
value="0.000001" />
<inertia
ixx="0.155943555803937"
ixy="-2.42861286636753E-17"
ixz="8.02309607639273E-18"
iyy="0.155943555803937"
iyz="6.93889390390723E-18"
izz="0.310773464453391" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://two_degree_platform/meshes/plat_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://two_degree_platform/meshes/plat_link.STL" />
</geometry>
</collision>
<dynamic damping="0.7" friction="0.5"/>
</link>
<joint
name="platform_2_joint"
type="fixed">
<origin
xyz="0 -0.0278 0.057"
rpy="0 0 -1.5708" />
<parent
link="platform_1_link" />
<child
link="platform_2_link" />
<axis
xyz="0 0 0" />
</joint>
<!-- motor -->
<transmission name="steering_gear_1_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="steering_gear_2_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="joint_1_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>100</mechanicalReduction>
</actuator>
</transmission>
<transmission name="steering_gear_2_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="platform_1_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="joint_2_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>100</mechanicalReduction>
</actuator>
</transmission>
<!-- ros_control -->
<gazebo>
<plugin name="gazebo_ros_control_platform" filename="libgazebo_ros_control.so">
<robotNamespace>/two_degree_platform</robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
<legacyModeNS>true</legacyModeNS>
</plugin>
</gazebo>
<gazebo reference="front_left_wheel_link">
<mu1>0.8</mu1>
<mu2>0.8</mu2>
<kp>10000000.0</kp>
<kd>1.0</kd>
<minDepth>0.001</minDepth>
<maxVel>0.1</maxVel>
<fdir1>1 0 0</fdir1>
<material>Gazebo/Grey </material>
</gazebo>
<gazebo reference="front_right_wheel_link">
<mu1>0.8</mu1>
<mu2>0.8</mu2>
<kp>10000000.0</kp>
<kd>1.0</kd>
<minDepth>0.001</minDepth>
<maxVel>0.1</maxVel>
<fdir1>1 0 0</fdir1>
<material>Gazebo/Grey </material>
</gazebo>
<gazebo reference="right_rear_link">
<mu1>0.8</mu1>
<mu2>10</mu2>
<kp>10000000.0</kp>
<kd>1.0</kd>
<minDepth>0.001</minDepth>
<maxVel>0.1</maxVel>
<fdir1>1 0 0</fdir1>
<material>Gazebo/Grey </material>
</gazebo>
<gazebo reference="left_rear_link">
<mu1>0.8</mu1>
<mu2>10</mu2>
<kp>10000000.0</kp>
<kd>1.0</kd>
<minDepth>0.001</minDepth>
<maxVel>0.1</maxVel>
<fdir1>1 0 0</fdir1>
<material>Gazebo/Grey </material>
</gazebo>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotSimType>steer_bot_hardware_gazebo/SteerBotHardwareGazebo</robotSimType>
<legacyModeNS>false</legacyModeNS>
</plugin>
</gazebo>
</robot>
launch:
<?xml version="1.0"?>
<launch>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="true"/>
<arg name="headless" value="false"/>
<arg name="debug" value="false"/>
</include>
<include file="$(find hunter2_control)/launch/hunter2_v_contol.launch" />
<param name="robot_description" command="$(find xacro)/xacro '$(find hunter2_base)/urdf/hunter2_base_gazebo.xacro'" />
<!-- push robot_description to factory and spawn robot in gazebo -->
<node name="hunter_spawn" pkg="gazebo_ros" type="spawn_model" output="screen" args="-urdf -param robot_description -model hunter2_base -z 1.5"
respawn="false" />
<rosparam file="$(find hunter2_base)/config/turtlebot3_platform.yaml" command="load"/>
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/two_degree_platform" args="joint_state_controller joint1_position_controller joint2_position_controller"/>
<node name="robot_state_publisher_platform" pkg="robot_state_publisher" type="robot_state_publisher"
respawn="false" output="screen">
<remap from="/joint_states" to="/two_degree_platform/joint_states" />
</node>
</launch>
二、知识点笔记
1. urdf、xacro、sdf区别
- urdf是老的gazebo模型格式,本身有一些缺陷,也缺一些功能。但是网上很多gazebo模型都是这个格式的。
- xacro是urdf的一个升级版,文件也是.urdf.xacro命名。加入了xacro:macro模板等功能,使得模型文件编写更方便。书写格式也和urdf稍有不同,本质和urdf一样。
- sdf是最新的gazebo模型和世界文件格式,有很多新增功能,也是未来的大势所趋。sdf官方github仓库中也提供了urdf转换为sdf的工具。未来urdf肯定会被淘汰,因此官方不再给出sdf转化为urdf的工具,而且很多插件和新功能也没法用urdf表示。urdf2sdf教程
2. 本文用到的xacro基本参数(基本是urdf的)
本文是从solidworks直接导出urdf然后直接转成xacro的,我感觉跟纯粹的xacro的语法应该不太一样,但是也能用?我只写一点基础的,我看得懂的。URDF 文件是一个标准的 XML 文件,在 ROS 中预定义了一系列的标签用于描述机器人模型,虽然机器人模型可能较为复杂,但是 ROS 的 URDF 中机器人的组成却是较为简单。
2.1 整体框架:
<?xml version="1.0" encoding="utf-8"?>
<!--robot标签作为根标签,关键参数为name-->
<robot
name="***">
<!--link 连杆标签,描述机器人的刚体结构,可以设计该部件的形状、尺寸、颜色、惯性矩阵、碰撞参数等一系列属性,这些属性一般可分为三大类:例形状、尺寸、颜色称为可视化部分,而惯性矩阵、碰撞参数单独分类。常见参数为name,inertial,visual,collision,dynamic-->
<link
name="***">
***
</link>
<!--joint 关节标签,描述机器人两个刚体或者坐标系之间的机械关系,常见参数为name,type,origin,parent,child-->
<joint
name="***"
type="***">
***
</joint>
<!--可以定义颜色或者插入ros_control插件-->
<gazebo>
***
</gazebo>
<!--可以定义关节运动的控制器,相当与在关节上定义一个电机?-->
<transmission name="***">
***
</transmission>
</robot>
2.2 link连杆标签官方文档
举例:
<link
name="plat_link">
<inertial>
<origin
xyz="1.02695629777827E-14 1.01363362148277E-12 -0.0149291851817156"
rpy="0 0 0" />
<mass
value="1.0" />
<inertia
ixx="0.155943555803937"
ixy="-2.42861286636753E-17"
ixz="8.02309607639273E-18"
iyy="0.155943555803937"
iyz="6.93889390390723E-18"
izz="0.310773464453391" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://two_degree_platform/meshes/plat_link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://two_degree_platform/meshes/plat_link.STL" />
</geometry>
</collision>
<dynamic damping="0.7" friction="0.5"/>
</link>
2.2.1 inertial
链接的质量、质心位置及其中心惯性特性。
origin
描述了连杆重心 C 相对于link坐标系的平移、旋转。若未指定则默认为identity
mass
链接的质量(单位:kg)
material
惯性矩阵
2.2.2 visual
链接的视觉属性。此元素指定对象的形状(盒子、圆柱体等),以用于可视化目的。
origin(可选)
视觉元素的参考坐标相对于本刚体的参考位姿(平移与旋转)。
geometry(必需)
视觉对象的形状。可以是:box长方体、cylinder圆柱、sphere球体、mesh网格
material(可选)
视觉元素的材质。允许在“链接”对象之外的顶层“机器人”元素中指定材质元素。然后,您可以从链接 元素中按名称引用材质。
2.2.3 collision
链接的碰撞属性。
2.3 joint关节标签官方文档
举例:
<joint
name="joint3"
type="fixed">
<origin
xyz="0 -0.0278 0.057"
rpy="0 0 -1.5708" />
<parent
link="link2" />
<child
link="plat_link" />
<axis
xyz="0 0 0" />
</joint>
2.3.1 name && type
joint其中主要表达了两个内容, 一个是name后面这个关节的名字, 还有一个就是type后面这个关节的类型。参考博客
关节一共有如下六个类型:
- continuous: 一个不受限制的, 绕着一根轴的转动副.
- revolute: 一个转动角度受到限制的, 绕着一根轴的转动副.
- prismatic: 一个沿着一根轴的滑动副, 并且有限位.
- fixed: 固定关节.
- floating: 这个关节允许六个自由度的运动, 浮动关节.
- planar: wiki上的原话是: This joint allows motion in a plane perpendicular to the axis. 我理解是, 这个关节允许再垂直于轴的一个平面内进行运动, 参考"ROS机器人开发实践"第115页上的内容. 这里的运动应该即包括平移, 也包括旋转.
2.3.2 origin
描述了从parent link
到child link
之间的一个变换.。
关节落在child link的原点(在solidworks绘制时注意这一点)。那么这个当中包含了两个属性:
xyz
(可选的: 默认为一个零向量): 这其实反映了两个坐标系之间的平移关系.
rpy
(可选的: 默认也是一个零向量): 这反映了两个坐标系之间旋转的一个关系, r代表了roll, 是绕着x轴旋转, p代表了pitch, 是绕着y轴旋转, y代表了yaw, 是绕着z轴旋转
2.3.3 parent && child
父系坐标系和子系坐标系
2.3.4 axis
相对于child link也就是joint link的坐标系:
-
对于continuous或者revolute关节, 反映了绕着哪一根轴旋转,
-
对于prismatic关节, 反映了沿着哪一根轴移动.
-
对于planar关节, 反映了沿着哪个平面(由法向量体现)移动.
2.3.5 calibration
用于校准关节的参考位置:
rising: 当关节沿正方向移动, 将除法上升沿.
falling: 反之.
2.3.6 dynamics
反映了这个关节的一些物理参数, 比如阻尼值或者静摩擦力等:
damping代表了阻尼, 单位为N·s/m
friction代表了静摩擦力. 单位为N
2.3.7 limit
仅仅在prismatic和revolute的关节时需要设置
其中包含如下几个属性:
lower和upper(可选, 默认为0): 反映了关节移动的最小值或者最大值. 如果关节是continuous类型的话, 就会被自动忽略. 如果不填的话全部默认为0.
effort(必须存在): 力度限制是限制标签的一个属性。在这种情况下,控制器不能对关节发出大于 30 N(旋转运动为 N-m)或小于 -30 N(旋转运动为 N-m)的指令。如果控制器试图发出超过力值限制的指令,关节将被截断。
3. gazebo模型网站
链接,一般的车、多旋翼、固定翼、船、各种世界环境都可以去上面找。可我的舵机平台就是没有^-^
三、 Error解决笔记
1.模型显示畸形
解决:yaml的PID参数不合适,改小