网站建设前言,一个空间可以放几个网站,深入解析wordpress pdf,网站页面关键字在哪里文章目录环境安装概述ros_control框架ros_control数据流文件配置附加工具故障问题解决参考接前两篇#xff1a;ROS MoveIT1#xff08;Noetic#xff09;安装总结 Solidworks导出为URDF用于MoveIT总结#xff08;带prismatic#xff09; MoveIT1 Assistant 总结 环境
Ubu…
文章目录环境安装概述ros_control框架ros_control数据流文件配置附加工具故障问题解决参考接前两篇ROS MoveIT1Noetic安装总结 Solidworks导出为URDF用于MoveIT总结带prismatic MoveIT1 Assistant 总结 环境
Ubuntu20.04ROS1 Noetic;VMware
安装
sudo apt-get install ros-noetic-joint-state-controller
sudo apt-get install ros-noetic-effort-controllers
sudo apt-get install ros-noetic-position-controllers
sudo apt-get install ros-noetic-joint-trajectory-controller
sudo apt-get install ros-noetic-controller-manager
sudo apt-get install ros-noetic-gazebo-ros-control
sudo apt-get install ros-noetic-ros-controllers概述
URDF 用于创建机器人模型、Rviz 可以显示机器人感知到的环境信息Gazebo 用于物理环境仿真。
先在Moveit!端配置关节和传感器接口yaml文件将其加载到rviz端再在机器人端配置ros_control和接口yaml文件将机器人加载到Gazebo。
最后同时启动加载有ros_control的Gazebo和加载有Moveit的rviz达到联合仿真的目的。
ros_control框架 ros_control包由以下几部分 combined_robot_hw硬件包一个允许将多个RobotHW组合成一个“RobotHW”的软件包。 controller_interfacecontroller接口 controller_managercontroller管理器提供了一个近乎实时的controller管理器用于管理加载、卸载、启停controllers。 controller_manager_msgcontroller管理器的消息类型定义了controller的状态消息类型msg以及调用controller_manager的服务类型srv。 hardware_interface硬件底层的接口向硬件发送(write())命令并从硬件接收(read())联合状态。 https://github.com/ros-controls/ros_control/wiki/hardware_interface joint_limits_interfacejoints限制接口根据URDF中的limit标签将joint limit载入到硬件层中。 transmission_interface传动接口根据URDF中的transmission标签将该关系载入到硬件层中。 realtime_tools实时控制工具:包含一组可以从硬实时线程中使用的工具而不会破坏实时行为。
ros_control数据流 Controller Manager每个机器人可能有多个controller所以这里有一个控制器管理器的概念提供一种通用的接口来管理不同的controller。controller manager的输入就是ROS上层应用的输出。 Controllercontroller可以完成每个joint的控制请求下层的硬件资源并且提供了PID控制器读取硬件资源接口中的状态在发布控制命令。 Hardware Rescource为上下两层提供硬件资源的接口。 RobotHW硬件抽象层和硬件直接打交道通过write和read方法来完成硬件的操作这一层也包含关节限位、力矩转换、状态转换等功能。 Real Robot实际的机器人上也需要有自己的嵌入式控制器接收到命令后需要反映到执行器上比如接收到位置1的命令后那就需要让执行器快速、稳定的到达位置1。
【控制流】 ROS中的Controller manager接收load_controller、unload_controller等命令来加载和运行不同类型的controller例如joint_position这些controller通过Hardware Resource接口向硬件抽象层RobotHW读取和发布控制命令这些命令再输入到机器人上的嵌入控制器上然后有执行器执行。
文件配置
/home/gw2/ws_moveit/src/assis_1/config/robot_controller.yaml中定义的是position_controllers/JointTrajectoryController则URDF中定义的transmission中如果使用的是PositionJointInterface必须要对应否则会提示找不到controller。 transmission nametrans_Joint1typetransmission_interface/SimpleTransmission/typejoint nameJoint1hardwareInterfacehardware_interface/PositionJointInterface/hardwareInterface/jointactuator nameJoint1_motorhardwareInterfacehardware_interface/PositionJointInterface/hardwareInterfacemechanicalReduction1/mechanicalReduction/actuator/transmissiontransmission nametrans_Joint2typetransmission_interface/SimpleTransmission/typejoint nameJoint2hardwareInterfacehardware_interface/PositionJointInterface/hardwareInterface/jointactuator nameJoint2_motorhardwareInterfacehardware_interface/PositionJointInterface/hardwareInterfacemechanicalReduction1/mechanicalReduction/actuator/transmissiontransmission nametrans_Joint3typetransmission_interface/SimpleTransmission/typejoint nameJoint3hardwareInterfacehardware_interface/PositionJointInterface/hardwareInterface/jointactuator nameJoint3_motorhardwareInterfacehardware_interface/PositionJointInterface/hardwareInterfacemechanicalReduction1/mechanicalReduction/actuator/transmissiontransmission nametrans_Joint4typetransmission_interface/SimpleTransmission/typejoint nameJoint4hardwareInterfacehardware_interface/PositionJointInterface/hardwareInterface/jointactuator nameJoint4_motorhardwareInterfacehardware_interface/PositionJointInterface/hardwareInterfacemechanicalReduction1/mechanicalReduction/actuator/transmissiontransmission nametrans_Joint5typetransmission_interface/SimpleTransmission/typejoint nameJoint5hardwareInterfacehardware_interface/PositionJointInterface/hardwareInterface/jointactuator nameJoint5_motorhardwareInterfacehardware_interface/PositionJointInterface/hardwareInterfacemechanicalReduction1/mechanicalReduction/actuator/transmission在urdf中加入gazebo的ros_control插件如果不加运行gazebo会显示机械臂都耷拉在地上仿佛电机没有使能一样。 gazeboplugin namegazebo_ros_control filenamelibgazebo_ros_control.soplugin namejoint_state_publisher filenamelibgazebo_ros_joint_state_publisher.sojointNameJoint1,Joint2,Joint3,Joint4,Joint5/jointName/pluginrobotNamespace//robotNamespace/plugin/gazebo检查ros_controllers.launch的argscontrol不要有空格。
修改ros_controllers.yaml
control:type: position_controllers/JointTrajectoryControllerjoints:- Joint1- Joint2- Joint3- Joint4- Joint5gains:Joint1: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 }Joint2: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 }Joint3: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 }Joint4: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 }Joint5: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 }运行rviz和gazebo:
source ~/ws_moveit/devel/setup.bash
roslaunch assis_1 demo_gazebo.launch可以看到Rviz中的运动在Gazebo中可以同步运动。 附加工具
rqt_graph 创建一个显示当前系统ROS程序运行情况的动态图形
安装
sudo apt install ros-noetic-rqt
sudo apt install ros-noetic-rqt-common-plugins运行
rosrun rqt_graph rqt_graph可以看到结果 通过这个图可以看到 /move_group发送/control/follow_joint_trajectory/goal【目标位置】到机器人机器人发送/joint_states【轴状态】到/move_group和/robot_state_publisher
rqt_joint_trajectory_controller 安装
sudo apt-get install ros-noetic-rqt-joint-trajectory-controller运行
roslaunch assis_1 demo_gazebo.launch 或 roslaunch assis_1 gazebo.launch
rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller效果拖动进度条可以在Gazebo实现各个关节的运动。 rqt_controller_manager rqt插件该插件以图形化方式加载卸载启动和停止控制器同时用来显示加载的控制器的信息。
安装
sudo apt-get install ros-noetic-rqt-controller-manager运行可以看到control和joint_state_controller两个控制器。
rosrun rqt_controller_manager rqt_controller_manager故障问题解决
Spawn service failed. Exiting. cmd /opt/ros/noetic/lib/gazebo_ros/gzserver -e ode worlds/empty.world parse as old deprecated model file failed. 这三个错误往往一起出现最后通过在urdf文件中添加解决 filename“libgazebo_ros_control.so” https://blog.csdn.net/qq_60018807/article/details/128543981 ERROR: cannot launch node of type [controller_manager/spawner]: controller_manager接着一堆错误。
sudo apt-get install ros-kinetic-controller-managerhttps://blog.csdn.net/weixin_45839124/article/details/106589576 模型自己转动乱跑
sudo apt-get install ros-noetic-gazebo-ros-control[ERROR] [1675950367.646886773, 0.307000000]: Failed to initialize the controller [ERROR] [1675950367.649888591, 0.308000000]: Initializing controller ‘control’ failed [ERROR] [1675950368.653177, 0.650000]: Failed to load control 在ros_controllers.yaml中添加前述代码。
[ERROR] [1675950605.984213275]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/Joint1 [ERROR] [1675950605.988813331]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/Joint2 [ERROR] [1675950605.991700327]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/Joint3 [ERROR] [1675950605.995635439]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/Joint4 [ERROR] [1675950605.999769977]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/Joint5
添加修改ros_controllers.yaml文件 /gazebo_ros_control: pid_gains:
control:type: position_controllers/JointTrajectoryControllerjoints:- Joint1- Joint2- Joint3- Joint4- Joint5/gazebo_ros_control:pid_gains:Joint1: {p: 12000, d: 50, i: 0.0, i_clamp: 10000 }Joint2: {p: 12000, d: 50, i: 0.0, i_clamp: 10000 }Joint3: {p: 12000, d: 50, i: 0.0, i_clamp: 10000 }Joint4: {p: 12000, d: 50, i: 0.0, i_clamp: 10000 }Joint5: {p: 12000, d: 50, i: 0.0, i_clamp: 10000 }但是加上以后发现Gazebo模型开始扭动起来没眼看。最终无视这个错误即可。 https://blog.csdn.net/qq_32896521/article/details/111143282?spm1001.2014.3001.5501 https://zhuanlan.zhihu.com/p/392635284 参考 https://blog.csdn.net/qq_34935373/article/details/95886151 https://ros-planning.github.io/moveit_tutorials/doc/gazebo_simulation/gazebo_simulation.html http://www.guyuehome.com/890 https://blog.csdn.net/qq_41035283/article/details/120572465 http://wiki.ros.org/ros_control?distronoetic