机械臂学习记录4—机械臂控制器

原创已于 2025-02-18 15:05:53 修改 · 2.1k 阅读

35 ·

本内容遵循CC 4.0 BY-SA版权协议

GEO检测

于 2025-02-17 21:25:19 首次发布

环境：ubuntu20.04 ros-noetic

一、Franka官方控制器使用

franka官方的franka_ros中的franka_example_controllers中提供了多种示例控制器，比如笛卡尔空间和关节空间的速度控制器、位置控制器、阻抗控制器等。但是需要注意的是，有些控制器是无法在仿真环境中使用的，因为部分控制器需要franka的实际硬件接口，而franka仿真中是没有提供这些硬件接口的。

1.urdf/xacro文件修改

在使用franka官方控制器之前，我们需要对urdf/xacro文件进行修改。

首先是对关节的<hardwareInterface>标签进行修改，这取决于你将要使用的控制器类型（位控还是力控之类的）。比如笔者之前使用的是moveit setup assistant配置的基础位置控制器position_controllers/JointTrajectoryController，那么该标签处的内容如下：

  <transmission name="left_arm_joint1_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint1_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>

后来笔者打算使用力控，于是修改标签如下：（注意：所有关节的这个都需要修改）

    <transmission name="${arm_id}_joint1_transmission">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="${arm_id}_joint1">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      </joint>
      <actuator name="${arm_id}_joint1_motor">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      </actuator>
    </transmission>

同时，可以新建一个xacro文件，命名为utils.xacro（这个文件其实就是franka_ros/franka_description/robots/utils.xacro中的，笔者截取了需要的部分）需要添加以下内容：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <!-- ========================================================== -->
  <!-- Adds the required tags to provide a `FrankaStateInterface` -->
  <!-- when simulating with franka_hw_sim plugin                  -->
  <!--                                                            -->
  <!-- arm_id - Arm ID of the panda to simulate (default 'panda') -->
  <!-- ========================================================== -->
  <xacro:macro name="transmission-franka-state" params="arm_id:=panda">
    <transmission name="${arm_id}_franka_state">
      <type>franka_hw/FrankaStateInterface</type>
      <joint name="${arm_id}_joint1"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
      <joint name="${arm_id}_joint2"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
      <joint name="${arm_id}_joint3"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
      <joint name="${arm_id}_joint4"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
      <joint name="${arm_id}_joint5"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
      <joint name="${arm_id}_joint6"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
      <joint name="${arm_id}_joint7"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>

      <actuator name="${arm_id}_motor1"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
      <actuator name="${arm_id}_motor2"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
      <actuator name="${arm_id}_motor3"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
      <actuator name="${arm_id}_motor4"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
      <actuator name="${arm_id}_motor5"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
      <actuator name="${arm_id}_motor6"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
      <actuator name="${arm_id}_motor7"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
    </transmission>
  </xacro:macro>

  <!-- ============================================================ -->
  <!-- Adds the required tags to provide a `FrankaModelInterface`   -->
  <!-- when simulating with franka_hw_sim plugin                    -->
  <!--                                                              -->
  <!-- arm_id - Arm ID of the panda to simulate (default 'panda')   -->
  <!-- root   - Joint name of the base of the robot                 -->
  <!-- tip    - Joint name of the tip of the robot (flange or hand) -->
  <!-- ============================================================ -->
  <xacro:macro name="transmission-franka-model" params="arm_id:=panda root:=panda_joint1 tip:=panda_joint7">
    <transmission name="${arm_id}_franka_model">
      <type>franka_hw/FrankaModelInterface</type>
      <joint name="${root}">
        <role>root</role>
        <hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
      </joint>
      <joint name="${tip}">
        <role>tip</role>
        <hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
      </joint>

      <actuator name="${root}_motor_root"><hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface></actuator>
      <actuator name="${tip}_motor_tip"  ><hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface></actuator>
    </transmission>
  </xacro:macro>
</robot>

在统合的xacro文件中如下调用：

<!-- 替换为自己的文件地址 -->
<xacro:include filename="$(find arm_manipulation)/urdf/utils.xacro" />

<xacro:transmission-franka-state arm_id="$(arg arm_id)" />
<xacro:transmission-franka-model arm_id="$(arg arm_id)"
     root="$(arg arm_id)_joint1"
     tip="$(arg arm_id)_joint8"
 />

其次是需要添加一个franka的硬件仿真，franka_gazebo/FrankaHWSim。添加这个能够给你提供两个franka硬件仿真接口franka_hw::FrankaModelInterface和franka_hw::FrankaStateInterface，这两个接口是使用franka官方控制器最基础的要求。

  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/</robotNamespace>
      <controlPeriod>0.001</controlPeriod>
      <robotSimType>franka_gazebo/FrankaHWSim</robotSimType>
    </plugin>
    <self_collide>true</self_collide>
  </gazebo>

如果你编译报错如下，那么就是说明你缺少使用该控制器相应的硬件接口，图片中的四个hardware_interface是最基础四个硬件接口。当你添加了franka_gazebo/FrankaHWSim后，理论上来说你是能提供一共6个硬件接口的。

上面笔者提到的franka官方有些控制器不能使用就是因为有些控制器需要一些额外的硬件接口，如果读者想要知道franka官方控制器具体哪些能够使用，可以查看每个控制器对应的头文件，比如cartesian_impedance_example_controller.h，在类声明的时候有以下内容，可以看到该控制器需要的接口是franka_hw::FrankaModelInterface、hardware_interface::EffortJointInterface和franka_hw::FrankaStateInterface，而我们是有这三种接口的，所有可以在仿真中使用。而比如说joint_impedance_example_controller.h，需要的接口是franka_hw::FrankaModelInterface、hardware_interface::EffortJointInterface和franka_hw::FrankaPoseCartesianInterface，其中franka_hw::FrankaPoseCartesianInterface我们是没法提供的，所以该控制器无法在仿真中使用。

class CartesianImpedanceExampleController : public controller_interface::MultiInterfaceController<
                                                franka_hw::FrankaModelInterface,
                                                hardware_interface::EffortJointInterface,
                                                franka_hw::FrankaStateInterface> {

class JointImpedanceExampleController : public controller_interface::MultiInterfaceController<
                                            franka_hw::FrankaModelInterface,
                                            hardware_interface::EffortJointInterface,
                                            franka_hw::FrankaPoseCartesianInterface> {

命名空间的问题：

当时还有一个比较困惑笔者我并且耗时较长的问题是这个命名空间的问题，貌似会发生打不开界面卡死的问题。这是因为控制器没有找到名称符合的机械臂模型，ROS的参数服务器上没有对应的模型。读者可以观察到我的插件gazebo_ros_control中的标签robotNamespace中是没有指定什么命名空间的，这是笔者多次尝试后的解决措施，同时还需要在launch文件中向参数服务器声明自己的模型名称，一定要与自己的模型名称相同，如下。如果读者还遇到什么奇怪的问题，建议多查看相关话题、参数服务器的参数，确保控制器中的配置参数和模型文件中相同。

<arg name="arm_id"      default="left_arm" doc="Name of the panda robot to spawn" />
<param name="arm_id" value="$(arg arm_id)"/>

2.修改xx_moveit_config文件中控制器相关配置文件

在使用franka提供的示例控制器前，需要明确他们使用时的名称，找到franka_ros中franka_example_controllers的franka_example_controllers_plugin.xml文件，franka将提供的所有控制器以插件形式生成，便于调用，在这里我们可以找到所需的控制器插件名称，比如想要使用上面提到的笛卡尔空间阻抗控制器：franka_example_controllers/JointImpedanceExampleController。

然后找到自己的xx_moveit_config中加载控制器配置的yaml文件，修改其中的控制器类型，如下。

cartesian_impedance_example_controller:
  type: franka_example_controllers/CartesianImpedanceExampleController
  arm_id: $(arg arm_id)
  joint_names:
    - $(arg arm_id)_joint1
    - $(arg arm_id)_joint2
    - $(arg arm_id)_joint3
    - $(arg arm_id)_joint4
    - $(arg arm_id)_joint5
    - $(arg arm_id)_joint6
    - $(arg arm_id)_joint7

3.使用franka提供的控制器

笔者使用的是franka的cartesian_impedance_example_controller控制器，通过向对应的话题发布机械臂末端目标点的笛卡尔空间的位姿控制机械臂运动，具体话题名称和消息类型，读者可以通过rostopic list和rosmsg show xxxx/xxxx查看。

二、如何自定义控制器

franka虽然提供一些示例控制器，但是有些时候无法满足我们的需求，这个时候就需要我们自定义控制器，以下简单介绍一下如何自定义控制器，并将其生成为插件形式，便于我们调用。（需要说明的是笔者是在工作空间下单独创建了一个新的功能包my_controllers）

1.创建my_controller.cpp以及对应的头文件

（1）程序结构

// 伪代码头文件
// include只是笔者所使用的一个示例，具体情况具体分析
#include <memory>
#include <string>
#include <vector>

#include <controller_interface/multi_interface_controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/robot_hw.h>
#include <realtime_tools/realtime_publisher.h>
#include <ros/node_handle.h>
#include <ros/time.h>
#include <franka_hw/franka_cartesian_command_interface.h>
#include <franka_hw/franka_model_interface.h>
#include <franka_hw/trigger_rate.h>
#include <franka_hw/franka_state_interface.h>
#include <Eigen/Dense>

namespace my_controllers {
    // 类的声明
    class 类名 : public controller_interface::MultiInterfaceController<
                                            franka_hw::FrankaModelInterface,
                                            hardware_interface::EffortJointInterface,
                                            franka_hw::FrankaStateInterface> {
        public:
        private:
    };
}

// 伪代码cpp文件
#include <my_controllers/joint_impedance_controller.h>
#include <cmath>
#include <controller_interface/controller_base.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <franka/robot_state.h>

namespace my_controllers
{
    类名::init();
    类名::starting();
    类名::update();
    类名::targetCellback();

}
PLUGINLIB_EXPORT_CLASS(my_controllers::类名,
                       controller_interface::ControllerBase)

（2）init函数

作用: 控制器初始化，完成参数读取、接口获取等准备工作
调用时机: 控制器启动时被调用一次
主要工作:
- 读取参数，比如arm_id、joint_names等参数
- 获取model interface、state interface等硬件接口
- 初始化变量和数组
- 订阅相关话题

（3）starting函数

作用: 控制器正式启动时的准备
调用时机: 控制器启动时，在init之后被调用一次
主要工作:
- 设置初始状态，比如设置初始期望关节位置为当前实际位置
- 初始化其他状态变量
- 输出启动日志信息

（4）update函数

作用: 控制器核心逻辑，计算输出数据
调用时机: 实时控制循环中，按固定频率周期性调用
主要工作:
- 计算需要输出的数据
- 更新控制器参数

（5）消息接收的回调函数

作用: 接收控制信息
调用时机: 发布者发布数据后自动调用
主要工作:
- 接收控制信息，比如目标位置，目标速度，目标力矩等

（6）使用PLUGINLIB_EXPORT_CLASS宏注册控制器

为了能够以插件形式使用控制器，需要在cpp文件的最后添加宏注册控制器。

PLUGINLIB_EXPORT_CLASS(my_controllers::JointImpedance,
                       controller_interface::ControllerBase)

2.新建一个名为xxxx.xml的文件

比如笔者我的就叫my_controllers_plugin.xml。其中，"lib/libxxxx"中xxxx要和命名空间相同，"my_controllers/类名"就是你的控制器插件名称。

<library path="lib/libmy_controllers">
  <class name="my_controllers/类名" type="my_controllers::类名" base_class_type="controller_interface::ControllerBase">
    <description>
    Impedance control in joint space with feed forward torques (for CMPs)
    </description>
  </class>
</library>

3.修改CMakeLists.txt和package.xml文件

在CMakeLists.txt的catkin_package中添加必要的包。（里面有些内容是使用franka所需添加的，读者视情况进行修改）

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES my_controllers
  CATKIN_DEPENDS 
    roscpp 
    rospy 
    std_msgs 
    geometry_msgs
    tf
    tf_conversions
    message_runtime
    realtime_tools
    controller_interface
    franka_hw
    eigen_conversions
    hardware_interface
    pluginlib
  DEPENDS 
          Franka
)

add_library(my_controllers
   src/joint_impedance_controller.cpp
)

add_dependencies(my_controllers
  ${${PROJECT_NAME}_EXPORTED_TARGETS}
  ${catkin_EXPORTED_TARGETS}
)

target_link_libraries(my_controllers
  ${Franka_LIBRARIES}
  ${catkin_LIBRARIES}
)

target_include_directories(my_controllers SYSTEM PUBLIC
  ${Franka_INCLUDE_DIRS}
  ${EIGEN3_INCLUDE_DIRS}
  ${catkin_INCLUDE_DIRS}
)
target_include_directories(my_controllers PUBLIC
  include
)

install(TARGETS my_controllers
  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
install(DIRECTORY launch
  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY config
  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(FILES my_controllers_plugin.xml
  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

在package.xml文件中添加以下内容：（里面有些内容是使用franka所需添加的，读者视情况进行修改）

<depend>controller_interface</depend>
<depend>franka_hw</depend>
<depend>hardware_interface</depend>
<depend>libfranka</depend>
<depend>pluginlib</depend>
<depend>tf</depend>
<depend>tf_conversions</depend>
<depend>realtime_tools</depend>

<exec_depend>franka_control</exec_depend>
<exec_depend>franka_description</exec_depend>
<exec_depend>message_runtime</exec_depend>

<!-- The export tag contains other, unspecified, tags -->
<export>
  <!-- Other tools can request additional information be placed here -->
  <controller_interface plugin="${prefix}/my_controllers_plugin.xml"/>
</export>

4.总结自定义控制器编写过程:

（1）继承ControllerBase类并实现必要接口

（2）在init中完成初始化工作

（3）在starting中完成启动准备工作

（4）在update中实现核心控制逻辑

（5）可添加其他辅助函数和回调函数

（6）使用PLUGINLIB_EXPORT_CLASS宏注册控制器

（7）编译并配置控制器参数文件

（8）在launch文件中加载和配置控制器

5.自定义控制器的使用

同前面介绍的使用franka官方示例控制器一样，修改控制器的配置文件，如果你的控制器还需要一些其他的参数，比如一些系数项，也需要在这里给定说明，比如笔者我的“k_gains”和“d_gains”。

arm_controller:
  type: my_controllers/JointImpedance
  arm_id: $(arg arm_id)
  joint_names:
    - $(arg arm_id)_joint1
    - $(arg arm_id)_joint2
    - $(arg arm_id)_joint3
    - $(arg arm_id)_joint4
    - $(arg arm_id)_joint5
    - $(arg arm_id)_joint6
    - $(arg arm_id)_joint7

  k_gains:
    - 150.0  # left_arm_joint1 的刚度
    - 150.0  # left_arm_joint2 的刚度
    - 150.0  # left_arm_joint3 的刚度
    - 150.0  # left_arm_joint4 的刚度
    - 150.0  # left_arm_joint5 的刚度
    - 150.0  # left_arm_joint6 的刚度
    - 50.0  # left_arm_joint7 的刚度
  d_gains:
    - 10.0    # left_arm_joint1 的阻尼
    - 10.0    # left_arm_joint2 的阻尼
    - 10.0    # left_arm_joint3 的阻尼
    - 10.0    # left_arm_joint4 的阻尼
    - 10.0    # left_arm_joint5 的阻尼
    - 10.0    # left_arm_joint6 的阻尼
    - 10.0    # left_arm_joint7 的阻尼

然后，只需要在运行程序中发布相应的话题信息就可以通过控制器控制机械臂运动。

三、关节阻抗控制器

笔者之前使用了franka提供的笛卡尔空间的阻抗控制器，但是由于我只能指定末端的位姿，其他关节的运动是由控制器内部的伪逆计算出的，这就意味着无法控制其他关节，那么多机械臂运动到各自目标位置的过程中就有可能会造成机械臂关节之间的碰撞，这是不被允许的，所以需要使用关节空间的阻抗控制器。但不幸的是，franka提供的关节阻抗控制器无法在仿真环境中使用，所以笔者只有自定义关节阻抗控制器。

1.阻抗控制

阻抗控制的具体原理不在此处赘述，网上有很多相关资料。关节空间的阻抗控制通过具体指定每个关节的关节角大小，计算每个关节的输出力矩大小，并且通过获取的coriolis对每个关节输出力矩进行补偿。

核心代码：

tau_d_calculated[i] = coriolis[i] +
                     k_gains_[i] * (q_d_[i] - robot_state.q[i]) +
                     d_gains_[i] * (dq_d_[i] - robot_state.dq[i]);

2.科里奥利补偿

coriolis数组包含了每个关节的科里奥利力和离心力项，用于补偿机械臂动态特性。

科里奥利力是由旋转运动产生的惯性力，在机械臂运动时不可忽略
离心力是由于关节旋转引起的作用于连杆的径向力
通过科里奥利补偿可以：
- 抵消这些非线性的动态效应
- 提高控制精度和系统响应速度
- 降低控制器增益要求
- 使系统更接近线性系统，便于控制

std::array<double, 7> coriolis = model_handle_->getCoriolis();

coriolis是通过franka获取的，其中model_handle_是定义在头文件中的一个指针。

std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;

标签

#学习 #机器人