运动学成本函数

在查询目标姿势或笛卡尔路径目标的 IK 解决方案时，我们可以指定将用于评估解决方案适用性的*成本函数*。

入门

如果您尚未完成此操作，请确保您已完成 入门 中的步骤。

本教程还需要使用 bio_ik 作为逆运动学插件。首先，将此存储库的“ros2”分支克隆到您的工作区的 src 目录中：:

git clone -b ros2 https://github.com/PickNikRobotics/bio_ik.git

然后，通过将以下内容复制到工作区内的 moveit_resources/panda_moveit_config/config/kinematics.yaml 中来更改 Panda 机器人的运动学插件：:

panda_arm:
    kinematics_solver: bio_ik/BioIKKinematicsPlugin
    kinematics_solver_search_resolution: 0.005
    kinematics_solver_timeout: 0.005
    kinematics_solver_attempts: 1
    mode: gd_c # use the gradient descent solver

进行这些更改后，重建您的工作区：:

colcon build --mixin release

运行代码

打开两个 shell。在第一个 shell 中，启动 RViz 并等待所有内容加载完成：:

ros2 launch moveit2_tutorials move_group.launch.py

在第二个 shell 中，运行教程启动文件：:

ros2 launch moveit2_tutorials kinematics_cost_function_tutorial.launch.py

要完成每个演示步骤，请按屏幕底部的 RvizVisualToolsGui 面板中的 Next 按钮，或选择屏幕顶部的 Tools 面板中的 Key Tool，然后在 RViz 处于焦点时按键盘上的 0。

预期输出

在 RViz 中，我们应该能够看到以下内容： 1. 机器人将其手臂移动到其右侧的姿势目标。在教程终端中记录了具有和不具有指定成本函数的关节运动的 L2 范数。 2. 机器人通过直线笛卡尔运动将其手臂移动到其左侧的姿势目标。

完整代码

完整代码可以在 MoveIt GitHub 项目 中看到。接下来，我们逐步介绍代码，以解释其功能。

Setup

MoveIt operates on sets of joints called “planning groups” and stores them in an object called the JointModelGroup. Throughout MoveIt, the terms “planning group” and “joint model group” are used interchangeably.

static const std::string PLANNING_GROUP = "panda_arm";

The MoveGroupInterface class can be easily set up using just the name of the planning group you would like to control and plan for.

moveit::planning_interface::MoveGroupInterface move_group(node, PLANNING_GROUP);

Raw pointers are frequently used to refer to the planning group for improved performance.

const moveit::core::JointModelGroup* joint_model_group =
    move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

Visualization

namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools(node, "panda_link0", "move_group_tutorial",
                                                    move_group.getRobotModel());

visual_tools.deleteAllMarkers();
visual_tools.loadRemoteControl();

Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.0;
visual_tools.publishText(text_pose, "KinematicsCostFn_Demo", rvt::WHITE, rvt::XLARGE);

visual_tools.trigger();

Start the demo

visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

Computing IK solutions with cost functions

When computing IK solutions for goal poses, we can specify a cost function that will be used to evaluate the “fitness” for a particular solution. At the time of writing this tutorial, this is only supported by the bio_ik kinematics plugin.

To start, we’ll create two pointers that references the current robot’s state. RobotState is the object that contains all the current position/velocity/acceleration data. By making two copies, we can test the difference between IK calls that do/don’t include cost functions

moveit::core::RobotStatePtr current_state = move_group.getCurrentState(10);
moveit::core::RobotStatePtr copy_state = move_group.getCurrentState(10);

We store the starting joint positions so we can evaluate performance later.

std::vector<double> start_joint_positions;
current_state->copyJointGroupPositions(joint_model_group, start_joint_positions);

Set a target pose that we would like to solve IK for

geometry_msgs::msg::Pose target_pose;
target_pose.orientation.w = 1.0;
target_pose.position.x = 0.28;
target_pose.position.y = -0.2;
target_pose.position.z = 0.5;

moveit::core::GroupStateValidityCallbackFn callback_fn;

Cost functions usually require one to accept approximate IK solutions

kinematics::KinematicsQueryOptions opts;
opts.return_approximate_solution = true;

Our cost function will optimize for minimal joint movement. Note that this is not the cost function that we pass directly to the IK call, but a helper function that we can also use to evaluate the solution.

const auto compute_l2_norm = [](std::vector<double> solution, std::vector<double> start) {
  double sum = 0.0;
  for (size_t ji = 0; ji < solution.size(); ji++)
  {
    double d = solution[ji] - start[ji];
    sum += d * d;
  }
  return sum;
};

The weight of the cost function often needs tuning. A tradeoff exists between the accuracy of the solution pose and the extent to which the IK solver obeys our cost function.

const double weight = 0.0005;
const auto cost_fn = [&weight, &compute_l2_norm](const geometry_msgs::msg::Pose& /*goal_pose*/,
                                                 const moveit::core::RobotState& solution_state,
                                                 moveit::core::JointModelGroup const* jmg,
                                                 const std::vector<double>& seed_state) {
  std::vector<double> proposed_joint_positions;
  solution_state.copyJointGroupPositions(jmg, proposed_joint_positions);
  double cost = compute_l2_norm(proposed_joint_positions, seed_state);
  return weight * cost;
};

Now, we request an IK solution twice for the same pose. Once with a cost function, and once without.

current_state->setFromIK(joint_model_group, target_pose, 0.0 /* timeout */, callback_fn, opts, cost_fn);
copy_state->setFromIK(joint_model_group, target_pose, 0.0 /* timeout */, callback_fn, opts);

std::vector<double> solution;
current_state->copyJointGroupPositions(joint_model_group, solution);

std::vector<double> solution_no_cost_fn;
copy_state->copyJointGroupPositions(joint_model_group, solution_no_cost_fn);

Now we can use our helper function from earlier to evaluate the effectiveness of the cost function.

double l2_solution = compute_l2_norm(solution, start_joint_positions);
RCLCPP_INFO_STREAM(LOGGER, "L2 norm of the solution WITH a cost function is " << l2_solution);
l2_solution = compute_l2_norm(solution_no_cost_fn, start_joint_positions);
RCLCPP_INFO_STREAM(LOGGER, "L2 norm of the solution WITHOUT a cost function is " << l2_solution);

If we’re happy with the solution, we can set it as a joint value target.

move_group.setJointValueTarget(solution);

We lower the allowed maximum velocity and acceleration to 5% of their maximum. The default values are 10% (0.1). Set your preferred defaults in the joint_limits.yaml file of your robot’s moveit_config or set explicit factors in your code if you need your robot to move faster.

move_group.setMaxVelocityScalingFactor(0.05);
move_group.setMaxAccelerationScalingFactor(0.05);

moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Visualizing plan 1 (joint space goal with cost function) %s", success ? "" : "FAILED");

Visualize the plan in RViz:

visual_tools.deleteAllMarkers();
visual_tools.publishAxisLabeled(target_pose, "goal_pose");
visual_tools.publishText(text_pose, "Joint_Space_Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory, joint_model_group);
visual_tools.trigger();

Uncomment if you would like to execute the motion

/*
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to execute the motion");
move_group.execute(my_plan);
*/

visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue.");

We can also specify cost functions when computing robot trajectories that must follow a cartesian path. First, let’s change the target pose for the end of the path, so that if the previous motion plan was executed, we still have somewhere to move.

target_pose.position.y += 0.4;
target_pose.position.z -= 0.1;
target_pose.orientation.w = 0;
target_pose.orientation.x = -1.0;
Eigen::Isometry3d target;
tf2::fromMsg(target_pose, target);

auto start_state = move_group.getCurrentState(10.0);
std::vector<moveit::core::RobotStatePtr> traj;
moveit::core::MaxEEFStep max_eef_step(0.01, 0.1);

Here, we’re effectively disabling the jump threshold for joints. This is not recommended on real hardware.

const auto jump_thresh = moveit::core::JumpThreshold::disabled();

The trajectory, traj, passed to computeCartesianPath will contain several waypoints toward the goal pose, target. For each of these waypoints, the IK solver is queried with the given cost function.

const auto frac = moveit::core::CartesianInterpolator::computeCartesianPath(
    start_state.get(), joint_model_group, traj, joint_model_group->getLinkModel("panda_link8"), target, true,
    max_eef_step, jump_thresh, callback_fn, opts, cost_fn);

RCLCPP_INFO(LOGGER, "Computed %f percent of cartesian path.", frac.value * 100.0);

Once we’ve computed the points in our Cartesian path, we need to add timestamps to each point for execution.

robot_trajectory::RobotTrajectory rt(start_state->getRobotModel(), PLANNING_GROUP);
for (const moveit::core::RobotStatePtr& traj_state : traj)
  rt.addSuffixWayPoint(traj_state, 0.0);
trajectory_processing::TimeOptimalTrajectoryGeneration time_param;
time_param.computeTimeStamps(rt, 1.0);

moveit_msgs::msg::RobotTrajectory rt_msg;
rt.getRobotTrajectoryMsg(rt_msg);

visual_tools.deleteAllMarkers();
visual_tools.publishTrajectoryLine(rt, joint_model_group);
visual_tools.publishText(text_pose, "Cartesian_Path_Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishAxisLabeled(target_pose, "cartesian_goal_pose");
visual_tools.trigger();

Once we’ve computed the timestamps, we can pass the trajectory to move_group to execute it.

move_group.execute(rt_msg);
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to end the demo.");

启动文件

整个启动文件位于 GitHub 上的 here。本教程中的所有代码都可以从 MoveIt 设置中的 moveit2_tutorials 包中运行。