代码：https://github.com/mrzhuzhe/Gurren

1. 概述

之前做了ROS moveit和Gazebo的demo，可以通过moveit生成末端执行器的到目标位置joint pose 或 cartesian pose的移动轨迹

Moveit 虽然可以考虑 (1)环境中的障碍物 (2)机器人本体 作路径规划，但是发现 moveit 的最小单元是轨迹点

也就是说： moveit 无法自定义轨迹之间两点的行为

因此：

• 1. 首先轨迹点应该是一个 catesian space（三维坐标系四元数姿态-用于几何变幻） 的描述 而非 joint space 描述（关节旋转位置描述-用于机器人）
  注： 机器人关节电机的位置 -> joint space -> cartesian space 位置
  
• 2. 根据机器人学导论《introduction to robotics》 - craig 轨迹点之间的运动应该是用forward/inverse kinematic dynamic 来求解，是非常核心的部分
• 3. 如果要自定义机器人实时的行为就得自定义ros_control，需要调用ros control loop 的钩子 start/update/shutup 等
• 4. ros control 才能对外部的信号(ros topic)做实时响应(Feedback)的逻辑
  ur5的控制频率是125hz ur5e 1000hz

所以：决定尝试自定义ros controller 做一个最小实现的baseline

2. 演示

• 1. 注册ros control
• 2. 位置控制 Joint to cartesian 和 cartesian to joint
• 3. 速度控制
• 4. 实现一个简单业务逻辑

3. 相关工作

参考书：《 Introduction to robotics 》 - craig 中文名：《机器人学导论》

3.1 背景知识：

• 0. joint space 和 cartesian space
  这俩个都是机器人状态的表示，其中cartesian指机器人末端的位置pose和朝向orientation
  joint space 指机器人关节扭动的角度
  一般我们会希望表示为机器人末端以一个速度移动，或者移动多少距离，cartesian space 描述更符合人的直觉
   
• 1. Eigen https://eigen.tuxfamily.org/
  Cpp 矩阵计算库， 用来创建dense或者稀疏矩阵，做各种矩阵相乘，小矩阵求解，求伪逆等
   
• 2. 运动学/逆运动学
  
  总的来说：就是如果已知机器人的 a/aplha/d： 关节位置/关节长度/关节偏置 就能根据机器人的安装位置逐关节的求解下一个关节的位置，也就是可以知道末端的位置，如上图二维的例子
  逆运动学：如果已知机器人“当前”和”目标“两个pose：可以是joint pose 也可以是cartesian pose，可以求这两个pose之间如果通过扭动机器人的关节来运动到目标位置
  此处可以参见《机器人学导论》第二章第三章
   
• 3. URDF
  机器人的模型定义：
  包含:
  关节位置/长度/偏置 信息来计算运动学
  关节重量，pid增益等
  关节的转动限制条件，极限速度，极限角度等
   
• 4. 实时反馈控制 和 Control loop
  Control loop 是 ros control 中约定的一个机器人控制的“生命周期钩子”
  例如：ur5 的频率是125hz，每0.01毫秒调用此函数钩子
  机器人可以在这个函数里获取到机器人当前的关节状态：速度位置等
  在钩子里也可以把外部的信息，如末端力sensor的信息wrench通过ros topic传进来
  所以如果要实时响应外部信号 就得在control loop 中写业务逻辑
   
• 5. ros 整体架构 和 gazebo 搭建仿真环境相关
  这块可以看上一篇博客 ROS moveit和Gazebo的demo

3.2 整体架构：

[TODO] 此处需要完善，补充图

• 1. firmware
  revolute joint: 伺服电机和减速机
• 2. ros controller joint cmmand interface ( joint state )
  ros 提供的硬件管理和命令“协议”的实现
• 3. kdl getHandles
  根据预先设置的机器人urdf初始化机器人joint space 和 cartesian space 转换关系
  并注册对机器人关节硬件的控制权
• 4. kdl forward/inverse solver
  在control loop 中调用求解器得到末端执行器运动，所需每个关节转动角度的方案，产生姿态变幻的运动
• 5. Business logic
  自定义的逻辑，例如力控，导纳，阻抗控制等

3.3 参考的代码库：

• 1. ros_control
  https://github.com/ros-controls/ros_control
  ros 控制器协议的核心概念
• 2. ros_controllers
  https://github.com/ros-controls/ros_controllers
  基于ros control 自定义的Joint space转换控制器 ，说实话这个库面向对象封装的非常的让人迷惑，如果你要做的cartesian控制器不用看这个库
• 3. FCI franka_ros 和 libfranka
  https://github.com/frankaemika/franka_ros
  franka 自己实现的一系列控制器，需要注意的是franka 支持effortjointcontroller，就是关节扭矩控制，但是ur是不支持关节扭矩控制的
• 4. ur compliant control
  https://github.com/MingshanHe/Compliant-Control-and-Application
  特别鸣谢我的实现大部分都基于这个库，按自己理解做了一些必要的简化
• 5. kdl_parser
  https://github.com/ros/kdl_parser
  用来解析urdf获得机器人运动学参数a/alpha/d等，很多库都依赖于这个
• 6. orocos kdl
  https://github.com/orocos/orocos_kinematics_dynamics
  运动学求解器 这个库极其重要，很多逆运动学的求解都依赖于它，但是文档确非常糟糕，我是慢慢看它的代码才了解了一点点
• 7. FZI cartesian controller
  https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers
  FZI 的开发者改进的运动学求解器，相当于自己实现了kdl的部分功能
• 8. moveit
  https://github.com/ros-planning/moveit
  其中求解逆运动学的部分基于orocos kdl但是也做了一些自己的修改

4. 逐步实现

• rqt
  
  rqt可以方便的调试：可以publish和和查看topic
  注意上图打勾的两个地方，左侧是publish topic 右侧是查看topic
   
• 注册控制器plugin
  // only test control hook Code: /my_controller/test_controller
  roslaunch ur_gazebo ur5e_bringup.launch controllers:=test_controller
  
  这块主要参考了ros_control本身的totoial
   
• 位置/和速度求解
  // cartesian position Code: /my_controller/cartesian_controller
  roslaunch ur_gazebo ur5e_bringup.launch controllers:=cartesian_controller
  
  这块需要用到下面这些kenimatic chain 速度/位置求解器
  // KDL definition: /my_controller/cartesian_controller/include/cartesian_controller/kdl_base.h
  KDL::ChainFkSolverVel;
  KDL::ChainFkSolverPos;
  KDL::ChainIkSolverVel;
  KDL::ChainIkSolverPos;
  
  这块kdl官方文档实在是年久失修，我一直找不到一个简单实现，
  直到被人推荐了https://github.com/MingshanHe/Compliant-Control-and-Application
  参照他调用kdl的例子结合kdl文档和代码才勉强把kdl用起来
  
   
• 速度求解
  // caertesian velocity controller Code: /my_controller/cartesian_velocity_controller
  roslaunch ur_gazebo ur5e_bringup.launch controllers:=cartesian_velocity_controller
  
  和位置一样速度也可以被分解为Jacobian，这是最符合人类直观感觉的一种控制方式
  但是这个模式由于关节的速度限制，奇点更多
  
   
• 响应外部topic做自己的业务逻辑
  // run to a position with static cartesian velocity Code: /my_controller/cartesian_velocity_position_controller
  roslaunch ur_gazebo ur5e_bringup.launch controllers:=cartesian_velocity_position_controller
  
  力控/阻抗控制/实时机器学习控制等
   

5. 讨论

遇到的问题

• 全局蓝图不清，知识点杂乱
• [TODO]

6. 后续工作

伺服电机

• 算法误差
• 伺服电机误差
• 自定义硬件方案

重写KDL

KDL的替代方案

• FZI cartesian controller
• IKfast
• trackIK