规划器代价

ISAAC教程合集地址: https://blog.csdn.net/kunhe0512/category_12163211.html

文章目录

• 规划器代价
• • 组件
  • 入门
  • 通过应用程序图自定义成本

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 to allow block operation to passed to this function.virtual void addGradient(double time, const VectorXd& state,Eigen::Ref gradient) = 0;// Adds the hessian of this cost function for the given state to the given matrix `hessian`.// `hessian` uses a Ref to allow block operation to passed to this function.virtual void addHessian(double time, const VectorXd& state,Eigen::Ref 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 functionvirtual 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 PlannerCostbool 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 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 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 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 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(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(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 functionPlannerCost* get() override {return static_cast(cost_.get());}// Name of the component implementating a PlannerCostBuilder to be used as distance functionISAAC_PARAM(std::string, component_name);// Constant multiplication factorISAAC_PARAM(double, constant, 20.0);private:std::unique_ptr 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)20.5*gain*min(0,distance(state)−targetdistance−alpha*speed)^20.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)"]
},

我们已经使用自定义构建器成功添加了一个新障碍。

更多精彩内容:
https://www.nvidia.cn/gtc-global/?ncid=ref-dev-876561