复合消息

在这里插入图片描述
ISAAC教程合集地址: https://blog.csdn.net/kunhe0512/category_12163211.html

文章目录

复合消息
- 测量类型
- 使用复合消息的示例
- - 手臂关节速度
- 基本轨迹命令
- 手臂关节和末端执行器命令
- CompositeMetric 和 CompositeAtlas
- - 组件
  - 使用 CompositeMetric 计算距离
  - 在 Python 脚本中创建 Cask
  - 将 FollowPath 与 CompositeAtlas 结合使用
- JSON 序列化
- - 图式

机器人硬件包括许多不同的部件，可以通过各种方式启动。示例包括微分基础、完整基础或由关节位置或关节速度控制的多关节臂。在 Isaac SDK 中，各种节点（如全局路径点规划器、局部轨迹规划器、控制器和驱动器）需要交换消息以传达机器人硬件的状态和命令。此外，不仅需要单个状态，有时还需要状态的时间序列、时间序列的批次或携带有关多个实例的信息的状态。

为了解决这种消息传递需求的多样性，Isaac SDK 提供了复合消息：一种可以解决上述所有用例的单一消息类型。

测量类型

复合消息可以包括以下测量类型：

类型	描述 \| 格式
`None`	数量的度量不包括在标量，矢量测量类型列表，因此不是指定的矩阵，张量。
`Time`	事件的时间，特别用于为时间序列存储时间戳	标量
`Mass`	实体的权重	千克[kg]
`Position`	表示位置的欧几里得向量空间中相对于参考的物体 frame：比如手机的位置房间里的机器人。该测量用于存储状态的一般意义 n维系统，例如关节多关节机器人手臂的“位置”。	n维向量米 [m] 或无单位 [1]
`Speed`	物体位置的变化率相对于随时间变化的参考系。这个测量也用于一般意义上存储一个状态的变化率 n维系统。	n维向量米/秒 [m/s] 或无单位/秒 [1/s]
`Acceleration`	速度的变化率。	n 维向量或标量米/秒/秒 [m/s^2] 或无单位/秒/秒 [1/s^2]
`Rotation`	实体在 2D 或 3D 空间中的旋转相对于锚点。轮换最好表示为单位复数 (cos(a), sin(a)) 或单位四元数。避免使用角度（对于 2D）或欧拉角（对于 3D）。	二维：归一化复数为 2 个标量三维：归一化四元数为 4 个标量
`AngularSpeed`	旋转随时间的变化率。	2D：标量； [rad/s] 或 [1/s] 3D：3维向量； [rad/s] 或 [1/s]
`AngularAcceleration`	角速度随时间的变化率	2D：标量； [rad/s^2] 或 [1/s^2] 3D：3维向量； [rad/s^2] 或 [1/s^2]

使用复合消息的示例

手臂关节速度

以下复合消息描述了机械臂的关节速度：

proto: {
  "schema": [
    {"entity": "base_2", "element_type": Float64, "measure": Speed},
    {"entity": "foo", "element_type": Float64, "measure": Position},
    {"entity": "elbow", "element_type": Float64, "measure": Speed},
    {"entity": "base_1", "element_type": Float64, "measure": Speed},
    {"entity": "wrist", "element_type": Float64, "measure": Speed}
  ],
  "schema_hash": "...",
  "values": {
    "element_type": Float64,
    "sizes": [4],
    "dataBufferIndex": 0
  }
}
buffers: [[0.3, 0.7, -0.4, 0.2, 0.1]]

该组件读取消息：


class MyArm4Controller {
 public:
  void start() {
    speed_parser_.requestSchema({{"base_1", "base_2", "elbow", "wrist"},
    CompositeParser::Speed});
  }
  void tick() {
    Vector4f speeds;
    if (!speed_parser_.parse(rx_command().getProto(), rx_command().buffers(), speeds)) {
      reportFailure("Could not parse message: %s", speed_parser_.error_str());
      return;
    }
  }
  ISAAC_RX(CompositeProto, command);
 private:
  CompositeParser speed_parser_;
};

输出向量是 [0.2, 0.3, -0.4, 0.1]

基本轨迹命令

以下复合消息包含一个基本的轨迹命令：

proto: {
  "schema": [,
    {"entity": "time", "element_type": Float64, "measure": Time},
    {"entity": "base", "element_type": Float64, "measure": Position},
    {"entity": "base", "element_type": Float64, "measure": LinearSpeed},
    {"entity": "base", "element_type": Float64, "measure": AngularSpeed}
  ],
  "schema_hash": "...",
  "values": {
    "element_type": Float64,
    "sizes": [5, 4],
    "dataBufferIndex": 0
  }
}
buffers: [[0.30, 17.4, 0.70, -0.40], [0.35, 17.8, 0.64, -0.38], [0.40, 18.2, 0.61, -0.36], [0.44, 18.5, 0.56, -0.34], [0.47, 18.7, 0.59, -0.31]]

该组件处理命令：

class MyTrajectoryReceiver {
 public:
  void start() {
    speed_parser_.requestSchema({{"base", LinearSpeed}, {"base", AngularSpeed}});
  }
  void tick() {
    Timeseries<Vector2f, float> series;
    if (!speed_parser_.parse(rx_command().getProto(), rx_command().buffers(), "time", series)) {
      reportFailure("Could not parse message: %s", speed_parser_.error_str());
      return;
    }
  }
  ISAAC_RX(CompositeProto, command);
 private:
  CompositeParser speed_parser_;
};

输出时间序列为：(0.30, [0.70, -0.40]), (0.35, [0.64, -0.38]), (0.40, [0.61, -0.36]) , (0.44, [0.56, -0.34]), (0.47, [0.59, -0.31])

手臂关节和末端执行器命令

以下复合消息包含用于手臂关节和末端执行器的命令。在这种情况下，末端执行器状态是二进制的，因此度量为 None：

proto: {
  "schema": [
    {"entity": "elbow", "element_type": Float64, "measure": Speed},
    {"entity": "base", "element_type": Float64, "measure": Speed},
    {"entity": "wrist", "element_type": Float64, "measure": Speed},
    {"entity": "gripper", "element_type": Float64, "measure": None}
  ],
  "schema_hash": "...",
  "values": {
    "element_type": Float64,
    "sizes": [5],
    "dataBufferIndex": 0
  }
}
buffers: [[0.3, 0.4, -0.2, 1]]

该组件处理命令：

class MyArmAndGripperController {
 public:
  void start() {
    arm_parser_.requestSchema({"base", "elbow", "wrist"}, CompositeParser::Speed);
    gripper_parser_.requestSchema({"gripper"}, CompositeParser::Position);
  }
  void tick() {
    Vector3d arm_joint_speeds;
    if (!arm_parser_.parse(rx_command().getProto(), rx_command().buffers(), arm_joint_speeds)) {
      reportFailure("Could not parse message: %s", parser_.error_str());
      return;
    }
    Vector1f gripper_position;
    if (!gripper_parser_.parse(rx_command().getProto(), rx_command().buffers(), gripper_position)) {
      reportFailure("Could not parse message: %s", parser_.error_str());
      return;
    }
  }
  ISAAC_RX(CompositeProto, command);
 private:
  CompositeParser arm_parser_;
  CompositeParser gripper_parser_;
};

输出是：arm_joint_speeds [0.4, 0.3, -0.2], gripper_position [1]

CompositeMetric 和 CompositeAtlas

CompositeProto 也可以用于表示配置空间中的路点，使用 cask 作为 atlas（一组命名的复合路点）的存储，使用 uuid 作为路点名称标识。必须定义距离度量来比较 CompositeProto 与航路点的“接近度”。

组件

isaac.composite.CompositeMetric 组件定义了如何计算两个复合原型之间的距离。它包含一个表示用于距离计算的数量的模式、用于每个数量的 p 范数及其在总距离中的权重。用例包括移动通过一组关节角度航路点或打开或关闭吸力末端执行器。

isaac.composite.CompositeAtlas 组件提供对存储在 cask 中的航路点的访问。这是支持从多个组件访问的线程安全方式。

isaac.composite.CompositePublisher 组件从 atlas 中读取路径点列表，并将整个路径发布为 CompositeProto。航路点都具有相同的模式，并在 CompositeProto 中表示为批次。

isaac.composite.FollowPath 组件从 CompositePublisher 接收路径并一次发布一个路标。它接收当前状态并计算到当前航路点的距离。一旦距离在公差范围内，就会发布下一个航路点。 CompositeMetric 组件必须附加到同一节点以指定如何计算距离。

使用 CompositeMetric 计算距离

下面的代码片段计算了命令和状态通道上两个输入原型消息之间的距离。请注意，如果小码使用 CompositeMetric 组件，则应将其放置在与该组件相同的节点中。

// get metric component
metric_ = node()->getComponent<CompositeMetric>();
// try to set schema for metric from command message
metric_->setOrLoadSchema(ReadSchema(tx_command().getProto()));
// get the schema used the CompositeMetric
const auto schema = metric_->getSchema();
// parse the two states to compare. error handle is omitted
if (schema) {
  parser.requestSchema(*schema)
  VectorXd x1(schema->getElementCount()),  x2(schema->getElementCount());
  if (parser.parse(tx_command().getProto(), tx_command().buffers(), x1) &&
      parser.parse(tx_state().getProto(), tx_state().buffers(), x2)) {
    double distance = *metric_->distance(x1, x2);
  }
}

在 Python 脚本中创建 Cask

下面的 Python 代码片段展示了如何创建一个带有复合路径点的Cask：

from engine.pyalice import Cask
from engine.pyalice.Composite import create_composite_message

cask = Cask(cask_root, writable=True)
msg = create_composite_message(quantities, values)
msg.uuid = name
cask.write_message(msg)

将 FollowPath 与 CompositeAtlas 结合使用

下面的 JSON 片段显示了如何创建跟随路径应用程序：

"graph": {
  "nodes": [
    {
      "name": "atlas",
      "components": [
       {
         "name": "CompositeAtlas",
         "type": "isaac:::composite::CompositeAtlas"
       }
     ]
    },
    {
      "name": "follow_path",
      "components": [
        {
          "name": "ledger",
          "type": "isaac::alice::MessageLedger"
        },
        {
          "name": "CompositeMetric",
          "type": "isaac:::composite::CompositeMetric"
        },
        {
          "name": "CompositePublisher",
          "type": "isaac::composite::CompositePublisher"
        },
        {
          "name": "FollowPath",
          "type": "isaac::composite::FollowPath"
        }
      ]
    }
  ],
  "edges": [
   {
     "source": "follow_path/CompositePublisher/path",
     "target": "follow_path/FollowPath/path"
   }
  ]
},
"config": {
  "follow_path": {
    "CompositePublisher": {
      "tick_period": "10Hz",
      "atlas": "atlas/CompositeAtlas",
      "path": ["cart_observe", "cart_align", "cart_dropoff"]
    },
    "CompositePublisher": {
      "tick_period": "10Hz",
      "wait_time": 1.0,
      "tolerance": 0.05
    }
  }
}

JSON 序列化

Measure、Quantity 和 Schema 的 JSON 序列化需要通过 ISAAC_PARAM 为 codelet 配置复合模式。

Measure：使用 CompositeProto::Measure 枚举的 capnp JSON 字符串序列化。由于名称空间限制，无法使用 NLOHMANN_JSON_SERIALIZE_ENUM。

// in codelet
ISAAC_PARAM(std::string, measure)
// in app.json configs
"measure": "angularSpeed"

Quantity：由实体（字符串）、度量（字符串）和可选维度（整数数组）的 JSON 数组组成。

// in codelet
ISAAC_PARAM(composite::Quantity, quantity)
// in app.json configs
"quantity": ["tool", "position", [3]]
// or
"quantity": ["elbow", "position"] // dimension default to [1]

图式

选项 1：数量清单

// in codelet
ISAAC_PARAM(composite::Schema, schema)
// in app.json configs
"schema": [["tool", "position", [3]], ["tool", "rotation", [4]], ["elbow", "speed"]]

选项 2：一组实体（字符串）和一个度量；尺寸默认为 [1]

// in codelet
ISAAC_PARAM(composite::Schema, schema)
// in app.json configs
"schema": {
  "entity": ["shoulder", "elbow", "wrist"],
  "measure": "speed"
}