31.Isaac教程--规划器代价
规划器代价
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 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 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 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 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 0.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