ros 机械臂学习之ros control 学习笔记 2

joint_trajectory_controller

接着上篇笔记聊该控制器。

- 轨迹表示
控制器采用模板化的方式，可以支持多种轨迹表达方式。
默认情况下采用了样条曲线插值法。
插值法的大致分为以下几类：
线性，三次，五次。（根据位置，速度，加速度的对应关系）
- 硬件接口类型
该控制器支持多种硬件接口类型。如针对关节的位置，速度以及力控制接口。
对于位置控制方式，目标位置（指令）直接面向关节。
然而对于速度或者是力控方式（关节），位置以及速度轨迹跟踪误差通过PID回路映射到速度（作用力）指令。就是说这里用到了pid进行调节。
可以从以下代码看到配置区别：

 head_controller:
  type: "position_controllers/JointTrajectoryController"
  joints:
    - head_1_joint
    - head_2_joint
head_controller:
  type: "velocity_controllers/JointTrajectoryController"
  joints:
    - head_1_joint
    - head_2_joint
  gains: # Required because we're controlling a velocity interface
    head_1_joint: {p: 100,  d: 1, i: 1, i_clamp: 1}
    head_2_joint: {p: 100,  d: 1, i: 1, i_clamp: 1}

注意：可以根据自己的需要定义新的硬件接口，只要满足或者是相似于上面提到的轨迹表达案例。（这里我的理解是只要你的trajectory消息类型和定义的匹配） 或者你也可以针对已支持的接口进行一个重映射（替换），例如你可以配置一个代理控制器用于生成力控命令。
其他需要考虑的点：
实时安全的实现。
正确处理缠绕（连续）接头。
对系统时钟变化的鲁棒性：不连续的系统时钟变化不会导致已经排队的轨迹段的执行不连续。

• 发送轨迹
  可用的接口：
  在ros中有两大机制用于发送轨迹给控制器：
  action interface 以及 topic interface 这里就是我们常说的话题方式以及action方式。
  如果allow_partial_joint_goal未设置为True，则两者都使用轨迹_msgs/JointTrajectory消息来指定轨迹，并要求为所有控制器关节指定值（而不是仅指定子集）。
  主要用action 方式发送，具体功能以及好处可以学习相关ros基础。
  Important note Even when a goal has been aborted, the controller will still attempt to execute the trajectory as best as possible.
  话题方式不存在实时反馈，发出去就不管了。尽管可以提供一定程度上的监控，通过query_state service and state topic （详情见ros api），但实现起来要比action麻烦的多。

• 抢先策略
  同一时刻只有1个action goal保持活跃状态，或者topic interfac使用的话就没有。
  路径与目标差异容忍值只针对active goal有效。
  如果一个活跃的active goal被另一个来自action或者话题接口的命令抢注，该目标会被取消并且客户端会被通知。
  提到了empty trajectory（来自topic interface）的内容。

• 轨迹替换
  1 joint trajectory 信息中通过header 中时间戳来设置一个开始执行的时间，默认为0表示现在开始。
  2 新到来的轨迹命令并不会立即抛弃当前的轨迹执行然后去执行新的。而是会结合两者的有效信息。
  这里给出了详细讨论的链接：

 http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement

总结为以下几点：
1 获取新轨迹的有用部分：获取新轨迹的way points，并进行替换。要是与之前轨迹中的way points没区别的话，就继续执行操作。
2 获取当前轨迹的有用部分： 保留当前轨迹直到新轨迹的开始时间，之后会丢弃后面的部分。
3 结合上述两部分。
具体例子介绍可见上述链接。

ros api

• Action interface
  control_msgs::FollowJointTrajectoryAction
  in the follow_joint_trajectory namespace of the controller.

# The joint trajectory to follow
trajectory_msgs/JointTrajectory trajectory

# Tolerances for the trajectory.  If the measured joint values fall
# outside the tolerances the trajectory goal is aborted.  Any
# tolerances that are not specified (by being omitted or set to 0) are
# set to the defaults for the action server (often taken from the
# parameter server).

# Tolerances applied to the joints as the trajectory is executed.  If
# violated, the goal aborts with error_code set to
# PATH_TOLERANCE_VIOLATED.
JointTolerance[] path_tolerance

# To report success, the joints must be within goal_tolerance of the
# final trajectory value.  The goal must be achieved by time the
# trajectory ends plus goal_time_tolerance.  (goal_time_tolerance
# allows some leeway in time, so that the trajectory goal can still
# succeed even if the joints reach the goal some time after the
# precise end time of the trajectory).
#
# If the joints are not within goal_tolerance after "trajectory finish
# time" + goal_time_tolerance, the goal aborts with error_code set to
# GOAL_TOLERANCE_VIOLATED
JointTolerance[] goal_tolerance
duration goal_time_tolerance

---
int32 error_code
int32 SUCCESSFUL = 0
int32 INVALID_GOAL = -1
int32 INVALID_JOINTS = -2
int32 OLD_HEADER_TIMESTAMP = -3
int32 PATH_TOLERANCE_VIOLATED = -4
int32 GOAL_TOLERANCE_VIOLATED = -5

# Human readable description of the error code. Contains complementary
# information that is especially useful when execution fails, for instance:
# - INVALID_GOAL: The reason for the invalid goal (e.g., the requested
#   trajectory is in the past).
# - INVALID_JOINTS: The mismatch between the expected controller joints
#   and those provided in the goal.
# - PATH_TOLERANCE_VIOLATED and GOAL_TOLERANCE_VIOLATED: Which joint
#   violated which tolerance, and by how much.
string error_string

---
Header header
string[] joint_names
trajectory_msgs/JointTrajectoryPoint desired
trajectory_msgs/JointTrajectoryPoint actual
trajectory_msgs/JointTrajectoryPoint error

• 订阅话题
  command (trajectory_msgs/JointTrajectory)
• 发布话题
  state (control_msgs/JointTrajectoryControllerState)
• 服务
  query_state (control_msgs/QueryTrajectoryState)
  这里只是简单的笔记记录，具体代码例子介绍可以见ros wiki相关页面。
  这里给一个minimal例子以及一个complete description例子。

到这里其实已经对控制器插件的配置已经有了很清晰的了解了。
可以看到有部分参数配置信息，以及它代表的作用含义。

  head_controller:
 type: "effort_controllers/JointTrajectoryController"
  joints:
    - head_1_joint
    - head_2_joint

  gains: # Required because we're controlling an effort interface
    head_1_joint: {p: 100,  d: 1, i: 1, i_clamp: 1}
    head_2_joint: {p: 100,  d: 1, i: 1, i_clamp: 1}
--------------------------------------------------------------------------
head_controller:
  type: "effort_controllers/JointTrajectoryController"
  joints:
    - head_1_joint
    - head_2_joint

  constraints:
    goal_time: 0.5                   # Override default
    stopped_velocity_tolerance: 0.02 # Override default
    head_1_joint:
      trajectory: 0.05               # Not enforced if unspecified
      goal: 0.02                     # Not enforced if unspecified
    head_2_joint:
      goal: 0.01                     # Not enforced if unspecified

  gains: # Required because we're controlling an effort interface
    head_1_joint: {p: 100,  d: 1, i: 1, i_clamp: 1}
    head_2_joint: {p: 100,  d: 1, i: 1, i_clamp: 1}

  state_publish_rate:  25            # Override default
  action_monitor_rate: 30            # Override default
  stop_trajectory_duration: 0        # Override default