31.Isaac教程--规划器代价

阿里云国内75折回扣微信号：monov8

阿里云国际，腾讯云国际，低至75折。AWS 93折免费开户实名账号代冲值优惠多多微信号：monov8 飞机：@monov6

规划器代价

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

文章目录

规划器代价

导航本地规划器基于线性二次调节器 (LQR) 规划器。它通过生成最小化成本函数的轨迹来工作。不幸的是没有适用于所有应用程序的单一成本函数；因此为您自己的应用定制成本函数非常重要。这是 PlannerCost 和 PlannerCostBuilder 发挥作用的地方：

PlannerCost 提供了一个接口来生成成本函数：

class PlannerCost {
 public:
  // Returns true if the current state is valid.
  virtual bool isValid(double time, const VectorXd& state);

  // Returns the evaluation at a given state and time.
  virtual double evaluate(double time, const VectorXd& state) = 0;

  // Adds the gradient of this cost function for the given state to the given vector `gradient`.
  // `gradient` uses a Ref<VectorXd> to allow block operation to passed to this function.
  virtual void addGradient(double time, const VectorXd& state,
                           Eigen::Ref<Eigen::VectorXd> gradient) = 0;

  // Adds the hessian of this cost function for the given state to the given matrix `hessian`.
  // `hessian` uses a Ref<VectorXd> to allow block operation to passed to this function.
  virtual void addHessian(double time, const VectorXd& state,
                          Eigen::Ref<Eigen::MatrixXd> hessian) = 0;
};

PlannerCostBuilder 提供了一个组件接口来将 Planner 成本添加到您的应用程序中：

class PlannerCostBuilder : public alice::Component {
 public:
  // Creates the cost function initially. Makes sure all necessary memory required for subsequent
  // calls to `update` is allocated.
  virtual PlannerCost* build() = 0;

  // Prepares the cost function for the given time interval.
  // Does not do any dynamic memory allocations.
  virtual void update(double start_time, double end_time) {}

  // Destroys the cost function and all memory which was allocated during `build`.
  virtual void destroy() = 0;

  // Returns a pointer to the maintained cost function
  virtual PlannerCost* get() = 0;
};

组件

主要组件是isaac.planner_cost.PlannerCostBuilder是新增PlannerCost的接口。

以下是已在 Isaac SDK 中实现并可供使用的成本函数：

isaac.planner_cost.AdditionBuilder：用来把几个成本加在一起。
isaac.planner_cost.SmoothMinimumBuilder：用于计算成本列表的最小值。它不是确切的最小值而是可微分的近似值。
isaac.planner_cost.RangeConstraintsCostBuilder：帮助您创建基于状态的二次成本。您可以定义最小值和最大值如果状态超出此范围将创建二次成本。
isaac.planner_cost.DistanceQuadraticCostBuilder：期望 PlannerCost 返回距离。它将根据目标距离和实际距离创建二次成本。
isaac.planner_cost.ObstacleDistanceBuilder：返回障碍物的有符号距离。它本身不是成本函数但可以与 isaac.planner_cost.DistanceQuadraticCostBuilder 一起使用来创建二次成本函数。
isaac.planner_cost.CirclesUnionSmoothDistanceBuilder：这是一个辅助函数用于为 RobotShape 的所有圆调用另一个 PlannerCost 函数。

入门

您可以运行 Flatsim 以查看 Navigation local planner 的执行情况：

bazel run //packages/flatsim/apps:flatsim -- --demo demo_1

如果您想创建自己的成本函数您应该首先确定 packages/planner_cost/gems 中的现有成本是否满足您的需要。如果这些成本都不够您将需要首先创建一个实现 PlannerCost 接口的类。

例如让我们看一下 ScalarMultiplication它将 PlannerCost 作为输入并将其乘以一个常数：

// This is an implementation of PlannerCost.
// It takes another PlannerCost and simply multiplies by a constant value.
class ScalarMultiplication : public PlannerCost {
 public:
  ScalarMultiplication(PlannerCost* cost, double constant) : cost_(cost), constant_(constant) {}

  // Returns true if the current state is valid. Here we will just rely on the other PlannerCost
  bool isValid(double time, const VectorXd& state) override {
    return cost_->isValid(time, state);
  }

  // Returns the evaluation at a given state and time.
  // We can multiply the result of cost_->evaluate() by our constant.
  double evaluate(double time, const VectorXd& state) override {
    return constant_ * cost_->evaluate(time, state);
  }

  // Adds the gradient of this cost function for the given state to the given vector `gradient`.
  // `gradient` uses a Ref<VectorXd> to allow block operation to passed to this function.
  // We need to scale the gradient by our constant.
  void addGradient(double time, const VectorXd& state, Eigen::Ref<VectorXd> gradient) override {
    VectorXd tmp_gradient = VectorXd::Zero(gradient.size());
    cost_->addGradient(time, state, tmp_gradient);
    gradient += tmp_gradient * constant_;
  }

  // Adds the hessian of this cost function for the given state to the given matrix `hessian`.
  // `hessian` uses a Ref<VectorXd> to allow block operation to passed to this function.
  // We need to scale the hessian by our constant.
  void addHessian(double time, const VectorXd& state, Eigen::Ref<MatrixXd> hessian) override {
    MatrixXd tmp_hessian = MatrixXd::Zero(hessian.rows(), hessian.cols());
    cost_->addHessian(time, state, tmp_hessian);
    hessian += tmp_hessian * constant_;
  }

 private:
  // Hold another cost_
  PlannerCost* cost_ = nullptr;
  double constant_ = 1.0;
};

获得新的 PlannerCost 后您可以使用自定义构建器如下所示。请注意它必须实现接口 PlannerCostBuilder：

class ScalarMultiplicationBuilder : public PlannerCostBuilder {
 public:
  // Creates the cost function initially. Makes sure all necessary memory required for subsequent
  // calls to `update` is allocated.
  PlannerCost* build() override {
    builder_ = node()->app()->findComponentByName<PlannerCostBuilder>(get_component_name());
    ASSERT(builder_ != nullptr,
           "Failed to load the component: %s", get_component_name().c_str());
    cost_.reset(new ScalarMultiplication(builder_->build(), get_constant()));
    return static_cast<PlannerCost*>(cost_.get());
  }

  // Prepares the cost function for the given time interval.
  // Does not do any dynamic memory allocations.
  void update(double start_time, double end_time) override {
    builder_->update(start_time, end_time);
  }

  // Destroys the cost function and all memory which was allocated during `build`.
  void destroy() override {
    cost_.reset();
    builder_->destroy();
  }

  // Returns a pointer to the maintained cost function
  PlannerCost* get() override {
    return static_cast<PlannerCost*>(cost_.get());
  }

  // Name of the component implementating a PlannerCostBuilder to be used as distance function
  ISAAC_PARAM(std::string, component_name);

  // Constant multiplication factor
  ISAAC_PARAM(double, constant, 20.0);

 private:
  std::unique_ptr<ScalarMultiplication> cost_;
  PlannerCostBuilder* builder_;
};

我们现在有一个新的 PlannerCost我们可以使用它来扩展任何现有的 PlannerCost。我们也有一个建造者。在下一节中我们将研究如何扩展现有导航图以扩展现有成本。

通过应用程序图自定义成本

要自定义图形请编辑 packages/navigation/apps/differential_base_control.subgraph.json 文件。

首先您应该找到包含所有构建器的节点：

{
  "name": "lqr_state_cost",
  "components": [
    {
      "name": "TotalSum",
      "type": "isaac::planner_cost::AdditionBuilder"
    },
    {
      "name": "LimitRange",
      "type": "isaac::planner_cost::RangeConstraintsCostBuilder"
    },
    {
      "name": "TargetRange",
      "type": "isaac::planner_cost::RangeConstraintsCostBuilder"
    },
    {
      "name": "SmoothMinimumBuilder",
      "type": "isaac::planner_cost::SmoothMinimumBuilder"
    },
    {
      "name": "CirclesUnionSmoothDistanceBuilder",
      "type": "isaac::planner_cost::CirclesUnionSmoothDistanceBuilder"
    },
    {
      "name": "ObstacleLocalMap",
      "type": "isaac::planner_cost::ObstacleDistanceBuilder"
    },
    {
      "name": "ObstacleRestrictedArea",
      "type": "isaac::planner_cost::ObstacleDistanceBuilder"
    },
    {
      "name": "DistanceQuadraticCostBuilder",
      "type": "isaac::planner_cost::DistanceQuadraticCostBuilder"
    }
  ]
},
{
  "name": "lqr_control_cost",
  "components": [
    {
      "name": "RangeConstraintsCostBuilder",
      "type": "isaac::planner_cost::RangeConstraintsCostBuilder"
    }
  ]
},

lqr_state_cost 包含用于计算与沿轨迹状态相关的成本的构建器列表而 lqr_control_cost包含与控制相关的成本。

再往下您可以找到与这些成本关联的配置参数：

"lqr": {
  "isaac.lqr.DifferentialBaseLqrPlanner": {
    ...
    "state_planner_cost_name": "$(fullname lqr_state_cost/TotalSum)",
    "control_planner_cost_name": "$(fullname lqr_control_cost/RangeConstraintsCostBuilder)"
    ...
  }
},

这里我们定义了与控制相关的成本的根和与状态相关的根：

对于控件我们有一个类型为 isaac.planner_cost.RangeConstraintsCostBuilder 的成本
对于状态根是 isaac.planner_cost.AdditionBuilder 类型这意味着我们将添加成本列表。查看 TotalSum 的配置我们可以找到添加了哪些成本：

"TotalSum": {
  "component_names": [
    "$(fullname lqr_state_cost/DistanceQuadraticCostBuilder)",
    "$(fullname lqr_state_cost/LimitRange)",
    "$(fullname lqr_state_cost/TargetRange)"
  ]
},

添加了三个成本来计算最终成本：

其中两个是 isaac.planner_cost.RangeConstraintsCostBuilder 类型。
最后一个是 isaac.planner_cost.DistanceQuadraticCostBuilder 类型。这是另一个递归调用它依赖于另一个 isaac.planner_cost.CirclesUnionSmoothDistanceBuilder 类型的 Builder它本身依赖于 isaac.planner_cost.SmoothMinimumBuilder 类型的 Builder它计算 isaac.planner_cost.ObstacleDistanceBuilder 列表的最小值：

"DistanceQuadraticCostBuilder": {
  "component_name": "$(fullname lqr_state_cost/CirclesUnionSmoothDistanceBuilder)"
},

"CirclesUnionSmoothDistanceBuilder": {
  "component_name": "$(fullname lqr_state_cost/SmoothMinimumBuilder)"
},

"SmoothMinimumBuilder": {
  "component_names": [
    "$(fullname lqr_state_cost/ObstacleLocalMap)",
    "$(fullname lqr_state_cost/ObstacleRestrictedArea)"
  ]
},

"ObstacleLocalMap": {
  "obstacle_name": "local_map"
},
"ObstacleRestrictedArea": {
  "obstacle_name": "map/restricted_area"
},

这乍一看可能很复杂——让我们从头开始分析：

ObstacleLocalMap 和 ObstacleRestrictedArea 都从 Atlas 加载障碍物并返回从 2d 到障碍物的有符号距离。
SmoothMinimumBuilder 有助于估算最小距离——最终我们想知道机器人离最近的障碍物有多近。如果你需要处理更多的障碍这将是一个添加的好地方。
CirclesUnionSmoothDistanceBuilder 是一个帮助计算距离的辅助函数不仅适用于单个 2d 点还适用于 SphericalRobotShape 中的所有圆。它将从障碍物列表中返回机器人的距离。
最后 isaac.planner_cost.DistanceQuadraticCostBuilder 需要一个距离函数并计算成本： $0.5*gain*min(0,distance(state)−targetdistance−alpha*speed)^2$ 。我们只需传递由 CirclesUnionSmoothDistanceBuilder 计算的距离函数。

让我们探讨如何修改上面的示例以添加自定义成本函数。假设您有以下内容：

一个新的 CustomDistanceBuilder它返回到某些障碍物的距离但以厘米为单位。
我们在上面定义的 ScalarMultiplicationBuilder。

现在我们需要结合两者来计算以米为单位的距离我们需要将它添加到障碍物列表中。首先我们需要将这两个组件添加到我们的节点：


{
  "name": "lqr_state_cost",
  "components": [
    {
      "name": "TotalSum",
      "type": "isaac::planner_cost::AdditionBuilder"
    },
    ...
    {
      "name": "DistanceQuadraticCostBuilder",
      "type": "isaac::planner_cost::DistanceQuadraticCostBuilder"
    },
    {
      "name": "ScalarMultiplicationBuilder",
      "type": "isaac::planner_cost::ScalarMultiplicationBuilder"
    },
    {
      "name": "CustomDistanceBuilder",
      "type": "isaac::planner_cost::CustomDistanceBuilder"
    }
  ]
},

之后我们需要为他们创建配置：

"lqr_state_cost": {
  ...
  "ScalarMultiplicationBuilder": {
    "component_name": "$(fullname lqr_state_cost/CustomDistanceBuilder)",
    "constant": 100.0
  },
  "CustomDistanceBuilder": {
    ...
  }
}

最后我们需要将新距离添加到现有障碍列表中：

"SmoothMinimumBuilder": {
  "component_names": [
    "$(fullname lqr_state_cost/ObstacleLocalMap)",
    "$(fullname lqr_state_cost/ObstacleRestrictedArea)",
    "$(fullname lqr_state_cost/ScalarMultiplicationBuilder)"
  ]
},