ROS笔记（三）用键盘控制模型在gazebo中移动

使用上一个笔记中的模型，实现在gazebo中用键盘控制移动。
熟悉ROS的通信方式。

经过一点时间的摸索和查找资料，发现模型设计还是有很多不合理的地方，比如腿的自由度、没有规定材料、减重等，目前只是为了熟悉ROS通信方式，且没有学习过步态规划，于是简化固定了一些关节，从每条腿的4自由度（髋关节垂直于身体旋转，髋关节前后摆动，膝关节前后摆动，足端沿小腿方向伸缩滑动），变成了2自由度（髋关节前后摆动，足端沿小腿方向伸缩滑动）。

在工作空间下有三个功能包。

其中mydog6_description是SolidWorks导出URDF文件夹，并且手动添加了base_link用于修正模型位子，将URDF改写为xarco，由于惯、和碰撞、关节限位、力矩、速度属性在SolidWorks导出URDF时已经生成，只需添加gazebo_ros_control插件和传动装置。
其中mydog6_dgazebo文件实现了模型在gazebo中的可视化，在上一笔记基础上还需要包含一个launch文件，使得gazebo订阅关节位置信息。

<include file="$(find mydog6_control)/launch/mydog6_control.launch" />

这个mydog6_control.launch在mydog6_control功能包的launch文件下。

<launch>

  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find mydog6_control)/config/mydog6_control.yaml" command="load"/>
 <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
	output="screen" ns="/mydog6" args="
            joint_state_controller
            joint1_position_controller
	        joint2_position_controller
            joint3_position_controller
            joint4_position_controller
            joint5_position_controller
            joint6_position_controller
            joint7_position_controller
            joint8_position_controller"/>

  <!-- convert joint states to TF transforms for rviz, etc -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
	respawn="false" output="screen">
        <remap from="/joint_states" to="/mydog6/joint_states" />
  </node>
  <!--node name="position_pub_node" pkg="mydog6_control" type="position_pub.py" respawn="true"/ -->

</launch>

其中mydog6_control.yaml内容：

mydog6:  
 # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 100  
  
  # Position Controllers ---------------------------------------
  joint1_position_controller:
    type: position_controllers/JointPositionController
    joint: Joint_lf_2
    #pid: {p: 100.0, i: 0.01, d: 10.0}
  joint2_position_controller:
    type: position_controllers/JointPositionController
    joint: Joint_rf_2
    #pid: {p: 100.0, i: 0.01, d: 10.0}
  joint3_position_controller:
    type: position_controllers/JointPositionController
    joint: Joint_lb_2
    #pid: {p: 100.0, i: 0.01, d: 10.0}
  joint4_position_controller:
    type: position_controllers/JointPositionController
    joint: Joint_rb_2
    #pid: {p: 100.0, i: 0.01, d: 10.0}
  joint5_position_controller:
    type: position_controllers/JointPositionController
    joint: Joint_lf_4
    #pid: {p: 100.0, i: 0.01, d: 10.0}
  joint6_position_controller:
    type: position_controllers/JointPositionController
    joint: Joint_rf_4
    #pid: {p: 100.0, i: 0.01, d: 10.0}
  joint7_position_controller:
    type: position_controllers/JointPositionController
    joint: Joint_lb_4
    #pid: {p: 100.0, i: 0.01, d: 10.0}
  joint8_position_controller:
    type: position_controllers/JointPositionController
    joint: Joint_rb_4
    #pid: {p: 100.0, i: 0.01, d: 10.0}

gazebo_ros_control/pid_gains:
  Joint_lf_2: {p: 100.0, i: 0.0, d: 10.0}
  Joint_rf_2: {p: 100.0, i: 0.0, d: 10.0}
  Joint_lb_2: {p: 100.0, i: 0.0, d: 10.0}
  Joint_rb_2: {p: 100.0, i: 0.0, d: 10.0}
  Joint_lf_4: {p: 2544.0, i: 0, d: 0.0}
  Joint_rf_4: {p: 2544.0, i: 0, d: 0.0}
  Joint_lb_4: {p: 2544.0, i: 0, d: 0.0}
  Joint_rb_4: {p: 2544.0, i: 0, d: 0.0}

是ros_control需要设置的一些参数，即发布关节状态、关节使用位置控制并设置PID。
启动gazebo环境：roslaunch mydog6_gazebo mydog6_world.launch
使用rostopic list 和info 可以看到gazebo已经订阅了话题。


我们只需要写对应的 publisher，将关节位子信息发出去，即可以控制gazebo中模型的运动。
要使用键盘控制模型，即需要一个节点1来发布按键信息，需要一个节点2来订阅节点1的信息，根据按下的键进行相应的关节信息的发布，所以节点2有两个功能，订阅键盘节点发布的消息和发布消息给gazebo。

其中pid_test节点是用来调PID用的，从gazebo仿真环境中获取关节实际控制弧度。
暂时还没学过步态规划，于是使用最简单的对角步态，用正弦函数驱动，最终可以简单的完成前后左右的运动。

存在的一些问题：
1.PID调节：
根据经典控制理论，系统要有明确的输入和输出。例如，对滑动关节进行位置控制，那么系统的输入量就是目标关节位置 θd​ ，输出量就是当前关节位置​ θ 。由于是电机驱动，而电机本身并不直接识别位置信号，而是通过电流来控制力矩。查看gazebo_ros_control插件目录下的src/default_robot_hw_sim.cpp，POSITION_PID情况下调用了Gazebo的SetForce()接口，因此该插件的确是以力/力矩作为作用信号的。
于是输入量和输出量转化为所需的关节力矩信号​，滑动关节单纯使用P控制时，动力学方程大概为：Kp*偏差=惯性力+阻尼力+重力。最开始调节按照自控理论来调试，P用优选法，再调I，发现效果都不好，波形存在杂波，有“尖峰”或“突跳”。经过分析，发现很可是是结构原因造成的（足端为滑动关节），受到重力等影响滑动关节力矩略有改变，于是有偏差，有偏差即控制，使滑动关节运动，使模型重心位子略有改变，力的作用而带来滑动关节随机的振荡，滑动关节伸出量越长，重心越高，滑动关节振荡幅度越大。表现为在gazebo中模型由于滑动关节振动会打滑。在gazebo中将模型推翻，使得滑动关节不负重（身体，大腿等刚体的重量），控制效果和理论一样，不会有突变的杂波，滑动关节也不会振动，可以确定干扰一部分来自自于结构的不合理。接下来还需要学习理论知识进行分析和改进。

2.经过实践和查询相关文献，发现模型设计的不合理，主要是足端滑动关节，本模型共四条腿且每条腿4个自由度，实际很多四足每条腿3个自由度，分别为髋关节上下前后和膝关节前后，并没有旋转。可以考虑改进模型，使用SolidWorks设计结构，每条腿使用3个自由度，而不再使用滑动关节。

体会：
从节点图可以看出来，让模型动起来并不算复杂，通过这个简单的实践，熟悉了模型的编写，除了手写以外还可以通过SolidWorks导出， 熟悉了仿真环境的搭建，并利用ROS原生的话题通信方式实现了模型的控制。ROS是一个很好的工具，具有良好的通信框架，它可以给我们搭建一个仿真平台，去验证一些想法，要实现一个模型的仿真涉及的方面很多，编程能力只是其中之一。理论知识也很重要，比如对角步态虽然是用的简单的sin驱动，但是由于缺少理论支撑，更多的时候是在调试，在用脑子想象运动的状态，让模型动起来更多的是在尝试。接下来的一段时间，需要学习一些运动学理论知识，若增加关节自由度，可能还需要设计规划步态和求运动学逆解。抓紧时候多看书。
对gazebo不是很理解，也不清楚参数配置是否正确，再花时间将gazebo教程完整跑一遍。