上一篇博客中，实现了gazebo中控制器的应用，但是由于PID调的不好，控制效果实在是不好意思贴出图来，作为一个控制理论与控制工程毕业的人，我怎么忍得了。今天就试着来编一编控制器。当然是参考了一些教程。12

ROS中有一些标准的控制器，比如joint state, position, trajectory，这些在gazebo仿真中也用到了一部分。

以下是控制器的工作流程

加载控制器后，第一个执行的函数就是初始化int()，这并不是启动控制器，初始化函数的参数包括机器人的状态参数和Nodehandle。初始化函数只执行一次。

启动控制器函数starting()，也只执行一次，用于控制器的启动。

update()函数保证了控制器的活跃状态。以1000Hz的频率执行代码，意味着一微秒就要计算完成一个执行周期。

最后stop()停止控制器命令也只执行一次。

建立一个控制器的过程需要以下几个步骤：

新建一个带有必要依赖的ROS包；
编写控制器C++代码；
将C++类注册或者发布为一个插件；
在XML文件中定义插件；
更新package.xml来发布插件；
编写CMakeLists.txt
编译代码
为控制器参数编写配置文件
在gazebo中进行仿真
使用controller manager加载控制器。

创建控制器功能包

$ catkin_create_pkg my_controller_pkg roscpp pluginlib controller_interface hardware_interface

建立一个控制器的头文件


#include <ros/node_handle.h>
#include <urdf/model.h>
#include <control_toolbox/pid.h>
#include <boost/scoped_ptr.hpp>
#include <boost/thread/condition.hpp>
#include <realtime_tools/realtime_publisher.h>
#include <hardware_interface/joint_command_interface.h>
#include <controller_interface/controller.h>
#include <control_msgs/JointControllerState.h>
#include <std_msgs/Float64.h>
#include <control_msgs/JointControllerState.h>
#include <realtime_tools/realtime_buffer.h>
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>namespace my_controller_pkg{class MyControllerClass: public controller_interface::Controller<hardware_interface::EffortJointInterface>
{
private:hardware_interface::JointHandle joint_;double init_pos_;ros::Subscriber sub_command_;public:/*** \brief Store position and velocity command in struct to allow easier realtime buffer usage*/struct Commands{double position_; // Last commanded positiondouble velocity_; // Last commanded velocitybool has_velocity_; // false if no velocity command has been specified};realtime_tools::RealtimeBuffer<Commands> command_;Commands command_struct_; // pre-allocated memory that is re-used to set the realtime buffervirtual bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n);virtual void starting();virtual void update(const ros::Time& time, const ros::Duration& period);virtual void stopping();void setCommandCB(const std_msgs::Float64ConstPtr& msg);/*!* \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)** \param command*/void setCommand(double pos_target);/*!* \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)*        Also supports a target velocity** \param pos_target - position setpoint* \param vel_target - velocity setpoint*/void setCommand(double pos_target, double vel_target);
};
}

建立代码文件

#include "my_controller_pkg/my_controller_file.h"
#include <pluginlib/class_list_macros.h>namespace my_controller_pkg {/// Controller initialization in non-realtime
bool MyControllerClass::init(hardware_interface::EffortJointInterface *robot,ros::NodeHandle &n)
{std::string joint_name;if (!n.getParam("joint_name", joint_name)){ROS_ERROR("No joint given in namespace: '%s')",n.getNamespace().c_str());return false;}joint_ = robot->getHandle(joint_name);
//  if (!joint_)
//  {//    ROS_ERROR("MyController could not find joint named '%s'",
//              joint_name.c_str());
//    return false;
//  }// Start command subscribersub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &MyControllerClass::setCommandCB, this);return true;
}/// Controller startup in realtime
void MyControllerClass::starting()
{init_pos_ = joint_.getPosition();
}/// Controller update loop in realtime
void MyControllerClass::update(const ros::Time& time, const ros::Duration& period)
{command_struct_ = *(command_.readFromRT());double command_position = command_struct_.position_;double desired_pos = init_pos_ + 0.5 * sin(ros::Time::now().toSec());double current_pos = joint_.getPosition();double commanded_effort = -1000 * (current_pos - command_position);//joint_.commanded_effort_ = -10 * (current_pos - desired_pos);joint_.setCommand(commanded_effort);}/// Controller stopping in realtime
void MyControllerClass::stopping()
{}void MyControllerClass::setCommandCB(const std_msgs::Float64ConstPtr& msg)
{setCommand(msg->data);
}
// Set the joint position command
void MyControllerClass::setCommand(double pos_command)
{command_struct_.position_ = pos_command;command_struct_.has_velocity_ = false; // Flag to ignore the velocity command since our setCommand method did not include it// the writeFromNonRT can be used in RT, if you have the guarantee that//  * no non-rt thread is calling the same function (we're not subscribing to ros callbacks)//  * there is only one single rt threadcommand_.writeFromNonRT(command_struct_);
}// Set the joint position command with a velocity command as well
void MyControllerClass::setCommand(double pos_command, double vel_command)
{command_struct_.position_ = pos_command;command_struct_.velocity_ = vel_command;command_struct_.has_velocity_ = true;command_.writeFromNonRT(command_struct_);
}} // namespace// Register controller to pluginlib
PLUGINLIB_EXPORT_CLASS(my_controller_pkg::MyControllerClass, controller_interface::ControllerBase);

其中hardware_interface::JointHandle可以参考：http://docs.ros.org/jade/api/hardware_interface/html/c++/classhardware__interface_1_1JointHandle-members.html

代码解释

等我以后插在代码注释里

建立插件描述文件

<library path="lib/libmy_controller_lib"><class name="my_controller_pkg/MyControllerClass" type="my_controller_pkg::MyControllerClass"           base_class_type="controller_interface::ControllerBase" /><description>My test.</description>
</library>

更新package.xml文件

<export><!-- Other tools can request additional information be placed here --><controller_interface plugin="${prefix}/controller_plugins.xml" />
</export>

更新CMakeLists.txt文件

## my_controller_file library
add_library(my_controller_lib src/my_controller_file.cpp)
target_link_libraries(my_controller_lib ${catkin_LIBRARIES})

建立控制器

使用catkin_make命令编译。然后用下列命令可以查看控制器是否编译成了一个插件

rospack plugins --attrib=plugin controller_interface

写控制器的配置文件

在上一文章的yaml基础上修改，让新编的控制器控制joint1

myfirstrobot:# Publish all joint states -----------------------------------joint_state_controller:type: joint_state_controller/JointStateControllerpublish_rate: 50  # Position Controllers ---------------------------------------joint2_position_controller:type: effort_controllers/JointPositionControllerjoint: joint2pid: {p: 120.0, i: 10.01, d: 20.0}# Position Controllers ---------------------------------------joint1_position_controller:type: my_controller_pkg/MyControllerClassjoint_name: joint1

其他跟上一博客中的内容一致。

在gazebo中应用

运行roslaunch myurdf gazebo.launch，会启动gazebo和rviz，运行以下命令，改变joint1的目标值

rostopic pub -1 /myfirstrobot/joint1_position_controller/command std_msgs/Float64 "data: 1"
rostopic pub -1 /myfirstrobot/joint1_position_controller/command std_msgs/Float64 "data: 0"

效果如下：

毕竟是个比例控制，不要有强迫症。
…………

当然作为一个控制专业毕业的，怎么能受得了，这次能看出超调吗？

扰动也不惧啊

Mastering ROS for Robotics Programming. ↩
https://github.com/ros-controls/ros_controllers ↩

为串联机械臂写一个ROS控制器相关推荐

UR3机械臂+Realsense D435+ROS手眼标定记录
UR3机械臂+Realsense D435+ROS手眼标定记录前面一段时间,由于实验室工作安排,上手了UR3机械臂和Realsense D435深度相机,在手眼标定这一段真的是让人心累,断断续续折腾 ...
在Simulink中利用simmechanics对三自由度的串联机械臂进行仿真
在写本科的毕业论文的时候,需要对三自由度的机械臂进行一个仿真实验.在网上查阅相关资料,得知可以利用simmechanics 来进行仿真实验.关于simmechanics 这个的介绍就百度一下就可以,这 ...
给自己的机械臂添加一个夹爪
我感觉大多数用moveit做机械臂控制的,在一开始,都是控制一个光杆机械臂,但是随着自己对ros和moveit学习的深入,会发现只控制一个光杆机械臂好像没什么意思嘛.最起码要给自己的机械臂加一个夹爪让 ...
基于matlab的mk2三自由度机械臂轨迹规划及控制器仿真设计
基于matlab的mk2三自由度机械臂轨迹规划及控制器仿真设计(报告+ppt) 摘要:本文的研究对象为EEZYbotARM MK2三自由度机械臂,分析了其机械结构,建立D-H参数表,同时在MATLA ...
kinova机械臂GEN3的Ros操作开发教程
文章目录 kinova GEN3 官方文件 kinova GEN3 ip设置 kinova GEN3 有线连接设置 windows下操作 ubuntu下连接操作: kinova GEN3 无线连接设置 ...
工程师的浪漫：用机械臂画一个爱心
目录 0 写在前面 1 生成爱心轨迹 2 机械臂逆运动学实现 3 实现机械臂画指定轨迹 3.1 读取数据 3.2 绘制机械臂 3.3 反解位姿 4 拓展 0 写在前面本文基于过去的博客平面2R机器人 ...
MoveIt的使用（二）机械臂URDF在ROS中MoveIt的配置和使用
一.机械臂在ros里面的可视化在SOLIDWORKS转换成urdf文件之后,生成的文件放在新建的功能包中后,编译(catkin_make),再执行下面代码即可运行可视化文件 roslaunch de ...
遨博机械臂——末端工具ROS驱动
文章目录知识目标 1. 机械臂末端工具(EOAT) 2. 电动夹爪 3. 气动吸盘参考知识目标学习机械臂常用末端工具构成: 学习aubo机械臂安装电动夹爪及启动吸盘的方法: 学习电动夹爪及气动 ...
【机器人学】基于PoE模型的串联机械臂UR5的正运动学、微分运动学和逆运动学
文章目录基本概念正运动学源码一阶运动学基本概念 PoE(Product of Exponential)指数积公式. PoE和DH的作用都是一样的. 实际使用过程中,绝大多数在售的机器人还都是 ...
使用ROS MoveIt!控制真实五自由度机械臂
文章目录使用ROS MoveIt!控制diy五自由度机械臂写在前面修改功能包中相关文件编写myrobot_controllers.yaml文件修改launch文件 myrobot_drive ...

为串联机械臂写一个ROS控制器