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