Kinematics Cost Functions

When querying for IK solutions for a goal pose or for a cartesian path goal, we can specify cost functions that will be used to evaluate the fitness of solutions.

Getting Started

If you haven’t already done so, make sure you’ve completed the steps in Getting Started.

This tutorial also requires the use of bio_ik as the Inverse Kinematics plugin. First, clone the “ros2” branch of this repository into your workspace’s src directory:

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

Then, change your kinematics plugin for the Panda robot by copying the following into moveit_resources/panda_moveit_config/config/kinematics.yaml within your workspace:

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

After making these changes, rebuild your workspace:

colcon build --mixin release

Running the Code

Open two shells. In the first shell, start RViz and wait for everything to finish loading:

ros2 launch moveit2_tutorials move_group.launch.py

In the second shell, run the tutorial launch file:

ros2 launch moveit2_tutorials kinematics_cost_function_tutorial.launch.py

To progress through each demo step either press the Next button in the RvizVisualToolsGui panel at the bottom of the screen or select Key Tool in the Tools panel at the top of the screen and then press 0 on your keyboard while RViz is focused.

Expected Output

In RViz, we should be able to see the following:

The robot moves its arm to the pose goal to its right. The L2 norm of the joint movement with and without the cost function specified is logged in the tutorial terminal.
The robot moves its arm via a straight cartesian movement to the pose goal at its left.

The Entire Code

The entire code can be seen here in the MoveIt GitHub project. Next, we step through the code piece by piece to explain its functionality.

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.");

The Launch File

The entire launch file is here on GitHub. All the code in this tutorial can be run from the moveit2_tutorials package that you have as part of your MoveIt setup.