ROS 2机器人操作系统与Gazebo机器人仿真（微课视频版）课件 第9章-其他类型机器人仿真简介 1-2节

ROS2机器人操作系统与Gazebo机器人仿真

第9章其它类型机器人仿真简介提纲9.1六足机器人仿真9.1.1建模9.1.2步态9.1.3仿真9.2四足机器人仿真 9.2.1建模 9.2.2仿真9.3双足机器人仿真9.3.1建模 9.3.2仿真9.4四旋翼无人机仿真9.4.1建模9.4.2仿真9.5海面船舶9.5.1建模 9.5.2仿真9.6水下潜艇

9.6.1建模9.6.2仿真9.7本章小结 Gazebo作为功能强大的物理仿真工具，提供了丰富的仿真功能，可以用于多种类型机器人的仿真。本章将介绍使用Gazebo和ROS2进行其它类型机器人仿真的方法和流程。一般来说，利用Gazebo和ROS2进行机器人联合仿真的一般流程是：（1）进行仿真环境和机器人的建模和调试；（2）创建仿真项目的ROS2功能包；（3）建立Gazebo和ROS2间的通信；（4）编写ROS2节点，完成仿真任务；（5）使用Launch文件启动各个节点，运行仿真。本章按照上述流程分别介绍利用Gazebo和ROS2进行六足、四足、双足、四旋翼无人机、水面船舶和水下潜艇等6种常见类型机器人仿真的步骤和方法，旨在说明上述使用Gazebo和ROS2进行机器人仿真的一般范式。9.1六足机器人仿真9.1六足机器人仿真六足机器人是一种具备六个足部的机器人，其主要特点是通过多个足的协调运动来实现稳定行走和适应复杂地形的功能。这种机器人设计模仿了动物（如蜘蛛）的行走方式，能够适应不同的地面环境，具备在陡峭的边坡、不平的路面以及复杂地形运动的能力。下图展示了一种六足机器人的外观。9.1六足机器人仿真六足机器人总共有六个足，每个足有三个转动关节构成，整个机器人的足部共计18个关节，也就是有18个自由度。六足机器人较多的足数和关节既有优点，也有不足，优点是其运动时总是可以保证至少三条腿着地，使得机器人始终具有静平衡，机器人不会因为重心不稳而跌倒。反之，六足机器人的缺点是，虽然较多的自由度使其运动具有非常强的灵活性，但在一定程度上对于控制和运动步态的设计带来了复杂性。一般对于六足机器人的研究主要集中于六足机器人的运动方式上，也就是步态的设计。以下介绍使用Gazebo和ROS2进行六足机器人步态仿真的方法和流程。9.1六足机器人仿真9.1.1建模六足机器人的建模需要考虑其腿部结构和关节分布。根据腿关节的转动方向，六足机器人可以分为六足狗和六足蜘蛛两种类型，如图所示。六足狗和六足蜘蛛都具有六个足，六足在身体两侧对称分布，每侧三个足，每个足都具有三个转动关节，不同之处在于六足狗与六足蜘蛛的关节转向不同，以至于二者在步态行走上有所差异。在建模时，可以使用简单的几何体（如立方体和圆柱体）来构建六足机器人的身体和腿部结构，这样既能保证模型的准确性，又能提升仿真效率。六足机器人的关节驱动可以采用SDF的位置关节控制器或轨迹关节控制器，以实现关节的运动控制。9.1六足机器人仿真9.1.1建模注意：在进行Gazebo机器人仿真时，许多初学者会在模型的视觉外观上投入过多精力，而忽视了仿真中的一些关键要素，如质量、转动惯量、摩擦系数等物理要素。此外，复杂的视觉模型可能会增加计算负担，导致仿真速度变慢。在不影响仿真结果的前提下，尽量简化模型的几何形状和纹理。9.1六足机器人仿真9.1.1建模（1）六足蜘蛛机器人建模六足蜘蛛在使用SDF建模时的主要要点：首先，选择一个合适的机器人姿态作为建模的参照，合适的姿态对杆件摆放时的定位在计算上具有十分显著的优势；然后，使用杆件<link>标签内的姿态子标签<pose>将杆件放置到选定的初始姿态；再次，按照连杆的父子关系创建类型为转动的关节，将各个连杆结合为一个六足蛛蛛机器人整体；最后，为各个关节添加关节控制器，并添加关节状态发布器。9.1六足机器人仿真9.1.1建模以下是六足蜘蛛机器人的SDF片段：①连杆。连杆构成了机器人的基本构件，连杆需要设置姿态、转动惯量、可视体积和碰撞体积等内容。<?xmlversion="1.0"?><sdfversion="1.11"><modelname="spiderbot"canonical_link="body"><self_collide>true</self_collide><linkname="body"><self_collide>true</self_collide><inertialauto="true"><density>3500</density></inertial>9.1六足机器人仿真9.1.1建模<collisionname="collision1"><geometry><box><size>0.20.40.1</size></box></geometry></collision><visualname="visual1"><geometry><box><size>0.20.40.1</size>9.1六足机器人仿真9.1.1建模</box></geometry><material><ambient>0.00.01.01</ambient><diffuse>0.00.01.01</diffuse><specular>0.00.01.01</specular></material></visual></link><linkname='l1leg1'><pose>-0.120.160000</pose><self_collide>true</self_collide>9.1六足机器人仿真9.1.1建模<inertialauto="true"><density>1000</density></inertial><collisionname="collision1"><geometry><cylinder><radius>0.02</radius><length>0.1</length></cylinder></geometry></collision><visualname="visual1">9.1六足机器人仿真9.1.1建模<geometry><cylinder><radius>0.02</radius><length>0.1</length></cylinder></geometry><material><ambient>0.01.01.01</ambient><diffuse>0.01.01.01</diffuse><specular>0.01.01.01</specular></material></visual></link>

（......此处省略其他连杆）9.1六足机器人仿真9.1.1建模②关节。关节将六足蜘蛛机器人的各杆件按相对运动关系连接在一起。关节采用转动类型的关节，并设置关节的相关参数。<jointname='l1leg1j'type='revolute'><pose>000000</pose><parent>body</parent><child>l1leg1</child><axis><xyz>001</xyz><limit><lower>-1.57</lower><upper>1.57</upper><effort>200</effort>9.1六足机器人仿真9.1.1建模<stiffness>100000000</stiffness><dissipation>1</dissipation></limit><dynamics><damping>0.8</damping><friction>0</friction><spring_reference>0</spring_reference><spring_stiffness>0</spring_stiffness></dynamics></axis></joint>（......此处省略其他关节）9.1六足机器人仿真9.1.1建模③关节控制器。关节控制器可以看作是向关节添加的驱动电机。此处，为了控制方便起见使用关节位置控制器，为每一个关节添加一个关节位置控制器，并使用默认的话题作为控制关节的接口。<pluginfilename="gz-sim-joint-position-controller-system"name="gz::sim::systems::JointPositionController"><joint_name>l1leg1j</joint_name><p_gain>100</p_gain><i_gain>1</i_gain><d_gain>10</d_gain><i_max>50</i_max><i_min>-50</i_min><cmd_max>300</cmd_max><cmd_min>-300</cmd_min></plugin>（......此处省略其他关节控制器）9.1六足机器人仿真9.1.1建模④关节状态发布器。关节状态发布器相当于位置传感器，默认情况下通过话题/joint_states发布机器人内所有关节的状态。关节的状态主要包含关节的当前角度、速度以及在关节上施加的力矩。<pluginname='gz::sim::systems::JointStatePublisher'filename='gz-sim-joint-state-publisher-system’/>以上就是六足蜘蛛机器人的建模方法。完成建模后，可在Gazebo中进行加载，使用JointPositionController插件测试关节的转动，以及通过话题查看/joint_states话题上输出的关节信息。测试模型成功后即可保存模型，并创建一个简单的机器人运行环境。注意：六足蜘蛛的完整代码参见随书附赠的代码，代码路径为ros2_ws9/src/spider/sdf/spider.sdf。本章其他类型机器人的源码均在名为ros2_ws9的文件夹内。9.1六足机器人仿真9.1.1建模（2）六足机器狗建模六足机器狗的建模与六足蜘蛛的建模基本上相同，惟一的区别是需要注意连杆的摆放姿态，以及关节的转动方向即可，而关节控制器和关节状态发布器与六足蜘蛛使用相同方式。六足机器狗的核心SDF代码如下：<?xmlversion="1.0"?><sdfversion="1.11"><modelname="sixlegdogs"canonical_link="body"><self_collide>true</self_collide><linkname="body"><self_collide>true</self_collide><inertialauto="true"><density>3500</density>9.1六足机器人仿真9.1.1建模</inertial><collisionname="collision1"><geometry><box><size>0.20.40.1</size></box></geometry></collision><visualname="visual1"><geometry><box><size>0.20.40.1</size></box>9.1六足机器人仿真9.1.1建模</geometry><!--let'saddcolortoourlink--><material><ambient>0.00.01.01</ambient><diffuse>0.00.01.01</diffuse><specular>0.00.01.01</specular></material></visual></link><linkname='l1leg1'>9.1六足机器人仿真9.1.1建模<posedegrees="true">-0.120.1609000</pose><self_collide>true</self_collide><inertialauto="true"><density>1000</density></inertial><collisionname="collision1"><geometry><cylinder><radius>0.02</radius><length>0.04</length></cylinder></geometry>9.1六足机器人仿真9.1.1建模</collision><visualname="visual1"><geometry><cylinder><radius>0.02</radius><length>0.04</length></cylinder></geometry><!--let'saddcolortoourlink--><material><ambient>0.01.01.01</ambient>9.1六足机器人仿真9.1.1建模<diffuse>0.01.01.01</diffuse><specular>0.01.01.01</specular></material></visual></link>

（......此处省略其他连杆）

<jointname='l1leg1j'type='revolute'><pose>000000</pose><parent>body</parent><child>l1leg1</child><axis><xyz>001</xyz><limit>9.1六足机器人仿真9.1.1建模<lower>-1.57</lower><upper>1.57</upper><effort>200</effort><!--force?--><velocity>3</velocity><!--setjointmaxspeedradians--><stiffness>100000000</stiffness><dissipation>1</dissipation></limit><dynamics>9.1六足机器人仿真9.1.1建模<damping>0.8</damping><friction>0</friction><spring_reference>0</spring_reference><spring_stiffness>0</spring_stiffness></dynamics></axis></joint>（......此处省略其他关节）<pluginfilename="gz-sim-joint-position-controller-system"name="gz::sim::systems::JointPositionController"><joint_name>l1leg1j</joint_name>9.1六足机器人仿真9.1.1建模<use_actuator_msg>true</use_actuator_msg><actuator_number>0</actuator_number><p_gain>100</p_gain><i_gain>1</i_gain><d_gain>10</d_gain><i_max>50</i_max><i_min>-50</i_min><cmd_max>300</cmd_max><cmd_min>-300</cmd_min></plugin>（......此处省略其他关节控制器）9.1六足机器人仿真9.1.1建模<pluginname='gz::sim::systems::JointStatePublisher'filename='gz-sim-joint-state-publisher-system'/></model></sdf>9.1六足机器人仿真9.1.2步态六足机器人的步态设计是其仿真研究的核心内容之一。六足机器人常见的步态包括三角步态、波形步态等。下面以三角步态为例介绍六足机器人的前进方法。三角步态是一种稳定的行走方式，通过交替抬起和放下三组腿部（每组三条腿），实现六足机器人在复杂地形上的平稳移动，如图所示。展示了六足机器人以三角步态的足部运动方式，（a）为初始状态，六个足着地均匀地放置在身体两侧，在运动开始后，（b）抬起一组腿，（c）向前摆动抬起的这组腿，摆动完成后，（d）放下摆动的腿，（e）抬起另一组腿，（f）将放下的腿向后摆，使机器人向前移动，（g）将抬起的腿向前摆动，摆动完成后，（h）放下摆动到位的腿，（i）抬起一组腿，并将放下的腿向后摆动，机器人向前移动，机器人状态恢复到状态（b），随后往复循环上述过程六足机器人便可向前移动。9.1六足机器人仿真9.1.2步态9.1六足机器人仿真9.1.3仿真（1）话题桥接。话题的桥接与模型中关节控制器的设置相关，在上述两个六足机器人中均使用了单个的关节控制器话题控制机器人。通过配置文件设置Gazebo与ROS2间每个关节的控制话题与关节状态话题的桥接，配置如下：-ros_topic_name:"l1leg1j"gz_topic_name:"/model/spider/joint/l1leg1j/0/cmd_pos"ros_type_name:"std_msgs/msg/Float64"gz_type_name:"ignition.msgs.Double"lazy:truedirection:ROS_TO_GZ#（此处省略其他17个关节的话题桥接配置）

9.1六足机器人仿真9.1.3仿真-ros_topic_name:"joint_states"gz_topic_name:"/world/spider/model/spider/joint_state"ros_type_name:"sensor_msgs/msg/JointState"gz_type_name:"ignition.msgs.Model"lazy:falsedirection:GZ_TO_ROS9.1六足机器人仿真9.1.3仿真（2）步态仿真。下面的ROS2节点通过简单的有限状态机，将三角步态分解为不同的状态，实现了六足机器人的三角步态前进，代码如下：importrclpyfromrclpy.nodeimportNodefromstd_msgs.msgimportFloat64fromsensor_msgs.msgimportJointStatefromgeometry_msgs.msgimportPoseArrayclassLeanstep(Node):def__init__(self):super().__init__('steplearner')self.joint_states=Noneself.subscription=self.create_subscription(9.1六足机器人仿真9.1.3仿真JointState,f'joint_states',self.currentpose,1)self.posesub=self.create_subscription(PoseArray,f'pose',self.getpose,1)self.jointscontroller={}forlegpartsinrange(3):forlegidxinrange(3):ltpname=f'l{legidx+1}leg{legparts+1}j'rtpname=f'r{legidx+1}leg{legparts+1}j'ltmppub=self.create_publisher(Float64,ltpname,1)rtmppub=self.create_publisher(Float64,rtpname,1)self.jointscontroller[ltpname]=ltmppubself.jointscontroller[rtpname]=rtmppub9.1六足机器人仿真9.1.3仿真self.states=self.getstates()self.get_logger().info('{}'.format((self.states)))self.curstate='unknow'self.timer=self.create_timer(1,self.timer_callback)#将定时器周期设为1

defcurrentpose(self,joint_states):self.joint_states=joint_states

defgetpose(self,pose):self.bodypose=pose.poses[0]9.1六足机器人仿真9.1.3仿真deftimer_callback(self):ifself.curstate=='unknow':st=self.states['stand']self.curstate='stand'elifself.curstate=='stand':st=self.states['leftlift']self.curstate='leftlift'elifself.curstate=='leftlift':st=self.states['leftgo']self.curstate='leftgo'elifself.curstate=='leftgo':st=self.states['leftdown']self.curstate='leftdown'9.1六足机器人仿真9.1.3仿真elifself.curstate=='leftdown':st=self.states['rightlift']self.curstate='rightlift'elifself.curstate=='rightlift':st=self.states['leftback']self.curstate='leftback'elifself.curstate=='leftback':st=self.states['rightgo']self.curstate='rightgo'elifself.curstate=='rightgo':st=self.states['rightdown']self.curstate='rightdown'9.1六足机器人仿真9.1.3仿真elifself.curstate=='rightdown':st=self.states['leftlift']self.curstate='leftlift'else:self.curstate='unknow'st=self.states['stand']self.get_logger().info('{}'.format((self.curstate)))self.run(st)defrun(self,state):forjointinstate:self.jointscontroller[joint].publish(Float64(data=float(state[joint])))9.1六足机器人仿真9.1.3仿真defgetstates(self):statesnames='stand','leftlift','leftgo','leftdown','rightlift','leftback','rightgo','rightdown','rightback'states=[]stand={'l1leg1j':0.0,'l2leg1j':0.0,'l3leg1j':0.0,'r1leg1j':0.0,'r2leg1j':0.0,'r3leg1j':0.0,'l1leg2j':-0.57,'l2leg2j':-0.57,'l3leg2j':-0.57,'r1leg2j':0.57,'r2leg2j':0.57,'r3leg2j':0.57,'l1leg3j':-1.0,'l2leg3j':-1.0,'l3leg3j':-1.0,'r1leg3j':1.0,'r2leg3j':1.0,'r3leg3j':1.0,}states.append(stand)9.1六足机器人仿真9.1.3仿真tmpd=self.liftup('left')tmp=stand.copy()tmp.update(tmpd)states.append(tmp)

tmpd=self.go('left')tmp=tmp.copy()tmp.update(tmpd)states.append(tmp)9.1六足机器人仿真9.1.3仿真tmpd=self.liftdown('left')tmp=tmp.copy()tmp.update(tmpd)states.append(tmp)tmpd=self.liftup('right')tmp=tmp.copy()tmp.update(tmpd)states.append(tmp)9.1六足机器人仿真9.1.3仿真tmpd=self.back()tmp=tmp.copy()tmp.update(tmpd)states.append(tmp)tmpd=self.go('right')tmp=tmp.copy()tmp.update(tmpd)states.append(tmp)9.1六足机器人仿真9.1.3仿真tmpd=self.liftdown('right')tmp=tmp.copy()tmp.update(tmpd)states.append(tmp)tmpd=self.back('right')tmp=tmp.copy()tmp.update(tmpd)states.append(tmp)returndict(zip(statesnames,states))9.1六足机器人仿真9.1.3仿真defstand(self):foriinrange(3):lname=f'l{i+1}leg2j'rname=f'r{i+1}leg2j'self.jointscontroller[lname].publish(Float64(data=-0.57))self.jointscontroller[rname].publish(Float64(data=0.57))foriinrange(3):lname=f'l{i+1}leg3j'rname=f'r{i+1}leg3j'self.jointscontroller[lname].publish(Float64(data=-1.0))self.jointscontroller[rname].publish(Float64(data=1.0))9.1六足机器人仿真9.1.3仿真defliftup(self,side='left'):ifside=='left':legs=['l2','r1','r3']else:legs=['l1','l3','r2']states={}forleginlegs:states[leg+'leg2j']=0.0returnstates9.1六足机器人仿真9.1.3仿真defliftdown(self,side='left'):ifside=='left':legs=['l2','r1','r3']else:legs=['l1','l3','r2']states={}forleginlegs:if'l'inleg:states[leg+'leg2j']=-0.57else:states[leg+'leg2j']=0.57returnstates9.1六足机器人仿真9.1.3仿真defgo(self,side='left'):ifside=='left':legs=['l2','r1','r3']else:legs=['l1','l3','r2']states={}forleginlegs:if'l'inleg:states[leg+'leg1j']=-0.2else:states[leg+'leg1j']=0.2returnstates9.1六足机器人仿真9.1.3仿真defback(self,side='left'):ifside=='left':legs=['l2','r1','r3']else:legs=['l1','l3','r2']states={}forleginlegs:if'l'inleg:states[leg+'leg1j']=0.2else:states[leg+'leg1j']=-0.2returnstates9.1六足机器人仿真9.1.3仿真defmain():rclpy.init(args=None)learnstep=Leanstep()try:rclpy.spin(learnstep)exceptException:learnstep.destroy_node()rclpy.shutdown()if__name__=='__main__':main()9.1六足机器人仿真9.1.3仿真（3）Launch文件。利用Launch文件整合Gazebo和ROS2的相关节点以启动和运行整个六足机器人仿真。Launch文件启动的节点有：使用ros_gz_sim功能包启动Gazebo仿真环境并添加六足机器人到仿真环境；使用ros_gz_bridge功能包建立Gazebo和ROS2间的话题桥接；使用robot_state_publisher功能包发布机器人的状态；启动RViz2节点；启动三角步态控制节点。9.1六足机器人仿真9.1.3仿真fromlaunchimportLaunchDescriptionfromlaunch.actionsimportIncludeLaunchDescriptionfromlaunch_ros.actionsimportNodefromlaunch.actionsimportDeclareLaunchArgumentfromlaunch.substitutionsimportLaunchConfigurationfromlaunch.launch_description_sourcesimportPythonLaunchDescriptionSourcefromament_index_pythonimportget_package_share_directoryimportosprint(__file__)packagepath=get_package_share_directory('spider')print(packagepath)sdfdir=packagepath+'/sdf/'print(sdfdir)9.1六足机器人仿真9.1.3仿真SDF_WORLD_PATH=sdfdir+'world.sdf'SDF_MODEL_PATH=sdfdir+'spider.sdf'

defloadsdfmodel(sdfmodelpath):withopen(sdfmodelpath)asf:robot_desc=f.read()robot_desc=robot_desc.replace('model://','file://'+sdfdir)returnrobot_desc

sdfmodelstr=loadsdfmodel(SDF_MODEL_PATH)defgenerate_launch_description():#launchagazeboworldforsimulationgazebo_launch_node=IncludeLaunchDescription(PythonLaunchDescriptionSource(9.1六足机器人仿真9.1.3仿真os.path.join(get_package_share_directory('ros_gz_sim'),'launch/gz_sim.launch.py')),launch_arguments=[('gz_args',SDF_WORLD_PATH+'-r')])

#addmodeltogazeborobot_spwan_node=Node(package='ros_gz_sim',executable='create',arguments=['-world','spider','-name','spider','-string',sdfmodelstr,'-x','0','-y','0','-z','0.25'])9.1六足机器人仿真9.1.3仿真#bridgegazebomsgtorosbridge_node=Node(package='ros_gz_bridge',executable='parameter_bridge',parameters=[{'config_file':sdfdir+'jointcontroltopicmap.yaml'},])#publishmodeltorosbyrobotstatepublisher,whichmakerobotcanbeseeninrviz2robot_state_node=Node(package='robot_state_publisher',executable='robot_state_publisher',name='robot_state_publisher',output='both',parameters=[{'use_sim_time':True},{'robot_description':sdfmodelstr},])9.1六足机器人仿真9.1.3仿真#startrviz2noderviz_node=Node(package='rviz2',executable='rviz2',parameters=[{'use_sim_time':True}],arguments=['-d',sdfdir+'spider.rviz'])

#robotcontrolnodelearnstep=Node(package='spider',executable='learnstep')returnLaunchDescription([gazebo_launch_node,robot_spwan_node,bridge_node,robot_state_node,rviz_node,learnstep])9.1六足机器人仿真9.1.3仿真功能包经过编译和安装后，启动六足机器人步态仿真，运行Launch文件，执行命令：ros2launchspiderspider_launch.py上述命令运行后，ROS2会启动Gazebo和RViz2开始仿真，六足机器人就会在Gazebo中使用三角步态向前移动，如图所示。注意：对于六足狗机器人可执行ros2_ws9工作空间下的sixrobot功能包中的sixcontroller_launch.py文件启动和运行仿真。9.2四足机器人仿真9.2四足机器人仿真四足机器人是指有四个足的机器人。四足机器人在形态和运动模式上通常仿生狗、牛、豹等动物的步态。相较于六足机器人，四足机器人具有较高的灵活性和适应性，能够在复杂地形中快速移动，同时保持较低的能量消耗。然而，由于其四足机器人拥有更少的足，在运动中保持其身体的稳定是一个重要的挑战，比六足机器人的步态更加复杂。因此，四足机器人的研究重点主要集中在步态学习与控制技术上，尤其是在动态环境下维持平衡的能力。目前四足机器人的步态控制研究已经取得显著进展，相关技术日趋成熟，部分企业已经推出了相关的产品。波士顿动力的Spot机器狗、麻省理工学院（MIT）的机器豹以及宇树科技的Go系列机器狗是其中的典型代表。这些产品不仅展示了四足机器人在科研领域的潜力，也为未来在物流配送、灾害救援、工业检查等领域的广泛应用奠定了基础。9.2四足机器人仿真9.2.1建模四足机器人的建模需要考虑其腿部结构和关节分布。简单的四足机器人可以通过几何体（如立方体和圆柱体）进行建模，以提高仿真效率。对于更加复杂精细的四足机器人，如宇树Go2，其建模需要考虑更多的细节，包括机器人的身体结构、腿部关节以及传感器的分布。以下分别介绍简单的四足机器人和宇树Go2四足机器人的建模方法。9.2四足机器人仿真9.2.1建模（1）简单的四足机器人四足机器人主要由身体和四条腿构成。四条腿固定在身体两侧，并且都具有相同的结构。每条腿由髋关节和膝关节构成，髋关节一般有两个自由度，在设计时通常分为左右摆动和前后摆动的两个子关节，膝关节可以前后摆动。图9.5展示了简单的四足机器人的建模效果。9.2四足机器人仿真9.2.1建模上图里简单的四足机器人的SDF代码如下：<?xmlversion="1.0"?><sdfversion="1.11"><modelname="fourlegdogs"canonical_link="base"><self_collide>true</self_collide><linkname="base"><self_collide>true</self_collide><inertialauto="true"><density>1500</density></inertial><collisionname="collision1"><geometry>9.2四足机器人仿真9.2.1建模<box><size>0.20.40.1</size></box></geometry></collision><visualname="visual1"><geometry><box><size>0.20.40.1</size></box></geometry><!--let's9.2四足机器人仿真9.2.1建模addcolortoourlink--><material><ambient>0.00.01.01</ambient><diffuse>0.00.01.01</diffuse><specular>0.00.01.01</specular></material></visual></link><linkname='l1leg1'><posedegrees="true">-0.120.1609000</pose><self_collide>true</self_collide>9.2四足机器人仿真9.2.1建模<inertialauto="true"><density>1000</density></inertial><collisionname="collision1"><geometry><cylinder><radius>0.02</radius><length>0.04</length></cylinder></geometry></collision><visualname="visual1">9.2四足机器人仿真9.2.1建模<geometry><cylinder><radius>0.02</radius><length>0.04</length></cylinder></geometry><!--let'saddcolortoourlink--><material><ambient>0.01.01.01</ambient><diffuse>0.01.01.01</diffuse><specular>0.01.01.01</specular>9.2四足机器人仿真9.2.1建模</material></visual></link>（......此处省略）

</model></sdf>9.2四足机器人仿真9.2.1建模（2）宇树Go2四足机器人下图展示了宇树Go2四足机器人的仿真效果。宇树Go2四足机器人与简单的四足机器人在运动结构上相同，有四条腿，每条腿有三个关节，外观上使用三维格网和纹理贴图提升了仿真的视觉效果。9.2四足机器人仿真9.2.1建模宇树Go2具有URDF模型文件，通过对URDF模型文件进行转换，得到宇树Go2的SDF模型，并通过添加关节控制器完成对Gazebo的适配。宇树Go2四足机器狗模型的代码如下：<?xmlversion="1.0"?><sdfversion="1.11"><modelname='go2'canonical_link="base">><linkname='base'><inertial><pose>0.0211893907265636310-0.0053716721074678611000</pose><mass>6.923</mass><inertia>9.2四足机器人仿真9.2.1建模<ixx>0.02450242076517968</ixx><ixy>0.00012166</ixy><ixz>0.0014956963870049491</ixz><iyy>0.098242939262173756</iyy><iyz>-3.1199999999999999e-05</iyz><izz>0.1071627184969941</izz></inertia></inertial><collisionname='base_collision'><pose>000000</pose><geometry>9.2四足机器人仿真9.2.1建模<box><size>0.376199999999999980.09350.114</size></box></geometry><surface><friction><ode/></friction><bounce/><contact/></surface></collision><collisionname='base_fixed_joint_lump__Head_upper_collision_1'><pose>0.2849999999999999800.01000</pose>9.2四足机器人仿真9.2.1建模<geometry><cylinder><length>0.089999999999999997</length><radius>0.050000000000000003</radius></cylinder></geometry><surface><friction><ode/></friction><bounce/><contact/></surface></collision>9.2四足机器人仿真9.2.1建模<collisionname='base_fixed_joint_lump__Head_lower_collision_2'><pose>0.292999999999999980-0.059999999999999998000</pose><geometry><sphere><radius>0.047</radius></sphere></geometry><surface><friction><ode/></friction>9.2四足机器人仿真9.2.1建模<bounce/><contact/></surface></collision><visualname='base_visual'><pose>000000</pose><geometry><mesh><scale>111</scale><uri>model://go2_dog/dae/base.dae</uri></mesh></geometry><material>9.2四足机器人仿真9.2.1建模<ambient>0.00.01.01</ambient><diffuse>0.00.01.01</diffuse><specular>0.00.01.01</specular></material></visual><pose>000000</pose><enable_wind>false</enable_wind></link>（......此处省略）</model></sdf>9.2四足机器人仿真9.2.1建模以上不论是简单的四足机器人还是宇树Go2四足机器人，二者的不同仅在于外观，关节的控制方式都采用了关节位置控制器，因此，二者具有相同的控制方法。9.2四足机器人仿真9.2.2仿真（1）话题桥接在四足机器人建模中，对于关节的控制方式采取了集成的方式，通过对关节编号，使用Actuators消息一次性控制多个关节，简化了机器人多关节控制时为每个关节添加话题的方法。在话题的桥接上，只需要一个话题就可完成所有关节的命令传输，以及另一个发布关节状态的话题，精简了话题的桥接数量，提高了桥接效率，代码如下：9.2四足机器人仿真9.2.2仿真-ros_topic_name:"actuators"gz_topic_name:"/actuators"ros_type_name:"actuator_msgs/msg/Actuators"gz_type_name:"gz.msgs.Actuators"lazy:falsedirection:ROS_TO_GZ-ros_topic_name:"joint_states"gz_topic_name:"/world/fourrobot/model/fourrobot/joint_state"ros_type_name:"sensor_msgs/msg/JointState"gz_type_name:"gz.msgs.Model"lazy:falsedirection:GZ_TO_ROS9.2四足机器人仿真9.2.2仿真（2）步态仿真四足机器人的步态仿真方法众多，难度也较大。以下给出一个基于有限状态机的四足机器人步态控制节点的实现，该节点能够控制四足机器人向前运动。importrclpyfromrclpy.nodeimportNodefromstd_msgs.msgimportFloat64fromactuator_msgs.msgimportActuatorsclassFourController(Node):def__init__(self):super().__init__('fourcontroller')self.curstate='unknow'9.2四足机器人仿真9.2.2仿真self.states=self.getstates()self.actuators_pub=self.create_publisher(Actuators,f'actuators',1)self.timer=self.create_timer(1,self.timer_callback)#将定时器周期设为1

deftimer_callback(self):ifself.curstate=='unknow':st=self.states['stand']self.curstate='stand'elifself.curstate=='stand':st=self.states['leftlift']self.curstate='leftlift'9.2四足机器人仿真9.2.2仿真elifself.curstate=='leftlift':st=self.states['rightdown']self.curstate='rightdown'elifself.curstate=='rightdown':st=self.states['rightlift']self.curstate='rightlift'elifself.curstate=='rightlift':st=self.states['leftdown']self.curstate='leftdown'elifself.curstate=='leftdown':st=self.states['leftlift']self.curstate='leftlift'9.2四足机器人仿真9.2.2仿真else:self.curstate='unknow'st=self.states['stand']self.get_logger().info('{}'.format((self.curstate)))self.run(st)

defrun(self,state):values_list=list(state.values())actuator=Actuators()actuator.position=values_listself.actuators_pub.publish(actuator)9.2四足机器人仿真9.2.2仿真defgetstates(self):statesnames='stand','leftlift','rightdown','rightlift','leftdown'states=[]stand={'l1leg1j':0.0,'l3leg1j':0.0,'r1leg1j':0.0,'r3leg1j':0.0,'l1leg2j':-0.78,'l3leg2j':-0.78,'r1leg2j':-0.78,'r3leg2j':-0.78,'l1leg3j':1.57,'l3leg3j':1.57,'r1leg3j':1.57,'r3leg3j':1.57,}states.append(stand)tmpd=self.liftup('left')tmp=stand.copy()tmp.update(tmpd)states.append(tmp)9.2四足机器人仿真9.2.2仿