common_utils.cpp

Go to the documentation of this file.

//
// Created by yijiangh on 7/5/17.
//

#include "common_utils.h"
#include "trajectory_utils.h"

#include <ros/topic.h>
#include <eigen_conversions/eigen_msg.h>

#include <descartes_planner/dense_planner.h>
#include <descartes_planner/sparse_planner.h>
#include <descartes_trajectory/axial_symmetric_pt.h>

#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/GetMotionPlan.h>

// services
#include <moveit_msgs/ApplyPlanningScene.h>
#include <moveit_msgs/GetPlanningScene.h>

#include <moveit_msgs/GetMotionPlan.h>

// mesh collision geometry import
#include <geometric_shapes/shape_operations.h>

// Constants
const static double DEFAULT_TIME_UNDEFINED_VELOCITY =
    0.0; // When a Descartes trajectory point has no timing info associated
// with it, this value (in seconds) is used instead
const static std::string DEFAULT_FRAME_ID =
    "world_frame"; // The default frame_id used for trajectories generated
// by these helper functions
const static double DEFAULT_ANGLE_DISCRETIZATION =
    M_PI / 12.0; // Default discretization used for axially-symmetric points
// in these helper functions
const static double DEFAULT_JOINT_WAIT_TIME = 5.0; // Maximum time allowed to capture a new joint
// state message
const static double DEFAULT_JOINT_VELOCITY = 0.3; // rad/s

// MoveIt Configuration Constants
const static int DEFAULT_MOVEIT_NUM_PLANNING_ATTEMPTS = 30;
const static double DEFAULT_MOVEIT_PLANNING_TIME = 20.0;   // seconds
const static double DEFAULT_MOVEIT_VELOCITY_SCALING = 0.5; // Slow down the robot
const static std::string DEFAULT_MOVEIT_PLANNER_ID = "RRTstar";
const static std::string DEFAULT_MOVEIT_FRAME_ID = "world_frame";
const static std::string DEFAULT_MOVEIT_PLANNING_SERVICE_NAME = "plan_kinematic_path";

const static std::string GET_PLANNING_SCENE_SERVICE = "get_planning_scene";
const static std::string GET_PLANNER_PARAM_SERVICE = "get_planner_params";

// customized threshold to get rid of weird solution generated by RRT (40)
const static int RRT_WEIRD_SOL_NUM_THRESHOLD = 40;

Eigen::Affine3d choreo_process_planning::createNominalTransform(const geometry_msgs::Pose& ref_pose,
                                                                  const geometry_msgs::Point& pt)
{
  Eigen::Affine3d eigen_pose;
  Eigen::Vector3d eigen_pt;

  tf::poseMsgToEigen(ref_pose, eigen_pose);
  tf::pointMsgToEigen(pt, eigen_pt);

  // Translation transform
  Eigen::Affine3d to_point;
  to_point = Eigen::Translation3d(eigen_pt);

  // Reverse the Z axis
  Eigen::Affine3d flip_z;
  flip_z = Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY());

  // "snap" to the pt and flip z axis
  return eigen_pose * to_point * flip_z;
}

Eigen::Affine3d choreo_process_planning::createNominalTransform(const geometry_msgs::Pose& ref_pose,
                                                                  const double z_adjust)
{
  Eigen::Affine3d eigen_pose;

  tf::poseMsgToEigen(ref_pose, eigen_pose);

  return createNominalTransform(eigen_pose, z_adjust);
}

Eigen::Affine3d choreo_process_planning::createNominalTransform(const Eigen::Affine3d &ref_pose,
                                                                  const double z_adjust)
{
  // Reverse the Z axis
  Eigen::Affine3d flip_z;
  flip_z = Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY());

  return ref_pose * Eigen::Translation3d(0, 0, z_adjust) * flip_z;
}

static double calcDefaultTime(const std::vector<double>& a, const std::vector<double>& b,
                              double max_joint_vel)
{
  ROS_ASSERT(a.size() == b.size());
  ROS_ASSERT(a.size() > 0);
  std::vector<double> result (a.size());
  std::transform(a.begin(), a.end(), b.begin(), result.begin(), [] (double a, double b)
  {
    return std::abs(a - b);
  });

  return *std::max_element(result.begin(), result.end()) / max_joint_vel;
}

trajectory_msgs::JointTrajectory choreo_process_planning::toROSTrajectory(
    const std::vector<std::vector<double>>& solution,
    const descartes_core::RobotModel& model)
{
  ros::Duration from_start(0.0);
  std::vector<double> joint_point;
  std::vector<double> dummy;
  trajectory_msgs::JointTrajectory ros_trajectory; // result

  const int dof = model.getDOF();

  for (std::size_t i = 0; i < solution.size(); ++i)
  {
    assert(solution[i].size()==dof);

    trajectory_msgs::JointTrajectoryPoint pt;
    pt.positions = solution[i];
    pt.velocities.resize(joint_point.size(), 0.0);
    pt.accelerations.resize(joint_point.size(), 0.0);
    pt.effort.resize(joint_point.size(), 0.0);

    if (i == 0)
    {
      from_start += ros::Duration(DEFAULT_TIME_UNDEFINED_VELOCITY); // default time
    }
    else
    {
      // If we have a previous point then it makes more sense to set the time of the
      // motion based on the largest joint motion required between two points and a
      // default velocity.
      const auto& prev = ros_trajectory.points.back().positions;
      const auto& next = pt.positions;
      const auto td = calcDefaultTime(prev, next, DEFAULT_JOINT_VELOCITY);
      from_start += ros::Duration(td);
    }

    pt.time_from_start = from_start;

    ros_trajectory.points.push_back(pt);
  }

  return ros_trajectory;
}

trajectory_msgs::JointTrajectory choreo_process_planning::toROSTrajectory(
    const choreo_process_planning::DescartesTraj& solution,
    const descartes_core::RobotModel& model)
{
  ros::Duration from_start(0.0);
  std::vector<double> joint_point;
  std::vector<double> dummy;
  trajectory_msgs::JointTrajectory ros_trajectory; // result

  for (std::size_t i = 0; i < solution.size(); ++i)
  {
    solution[i]->getNominalJointPose(dummy, model, joint_point);

    trajectory_msgs::JointTrajectoryPoint pt;
    pt.positions = joint_point;
    pt.velocities.resize(joint_point.size(), 0.0);
    pt.accelerations.resize(joint_point.size(), 0.0);
    pt.effort.resize(joint_point.size(), 0.0);

    double time_step = solution[i]->getTiming().upper; // request descartes timing
    if (time_step == 0.0)
    {
      if (i == 0)
      {
        from_start += ros::Duration(DEFAULT_TIME_UNDEFINED_VELOCITY); // default time
      }
      else
      {
        // If we have a previous point then it makes more sense to set the time of the
        // motion based on the largest joint motion required between two points and a
        // default velocity.
        const auto& prev = ros_trajectory.points.back().positions;
        const auto& next = pt.positions;
        const auto td = calcDefaultTime(prev, next, DEFAULT_JOINT_VELOCITY);
        from_start += ros::Duration(td);
      }
    }
    else
    {
      from_start += ros::Duration(time_step);
    }

    pt.time_from_start = from_start;

    ros_trajectory.points.push_back(pt);
  }

  return ros_trajectory;
}

void choreo_process_planning::fillTrajectoryHeaders(const std::vector<std::string>& joints,
                                                      trajectory_msgs::JointTrajectory& traj,
                                                      const std::string world_frame)
{
  traj.joint_names = joints;
  traj.header.frame_id = world_frame;
  traj.header.stamp = ros::Time::now();
}

void choreo_process_planning::appendTrajectoryHeaders(const trajectory_msgs::JointTrajectory& orig_traj,
                                                        trajectory_msgs::JointTrajectory& traj,
                                                        const double sim_time_scale)
{
  traj.joint_names = orig_traj.joint_names;
  traj.header.frame_id = orig_traj.header.frame_id;
  traj.header.stamp = orig_traj.header.stamp + orig_traj.points.back().time_from_start;

  // set time_from_start relative to first point
  ros::Duration base_time = traj.points[0].time_from_start;

  for (int i = 0; i < traj.points.size(); i++)
  {
    traj.points[i].time_from_start -= base_time;

    //sim speed tuning
    traj.points[i].time_from_start *= sim_time_scale;
  }
}

std::vector<double> choreo_process_planning::getCurrentJointState(const std::string& topic)
{
  sensor_msgs::JointStateConstPtr state = ros::topic::waitForMessage<sensor_msgs::JointState>(
      topic, ros::Duration(DEFAULT_JOINT_WAIT_TIME));
  if (!state)
    throw std::runtime_error("Joint state message capture failed");
  return state->position;
}

bool choreo_process_planning::addCollisionObject(
    ros::ServiceClient& planning_scene_diff_client, const moveit_msgs::CollisionObject& c_obj)
{
  if(planning_scene_diff_client.waitForExistence())
  {
//    ROS_INFO_STREAM("planning scene diff srv connected!");
  }
  else
  {
    ROS_ERROR_STREAM("cannot connect with planning scene diff server...");
  }

  moveit_msgs::ApplyPlanningScene srv;

  moveit_msgs::PlanningScene planning_scene_msg;
  planning_scene_msg.world.collision_objects.push_back(c_obj);
  planning_scene_msg.is_diff = true;
  srv.request.scene = planning_scene_msg;

  if(planning_scene_diff_client.call(srv))
  {
//    ROS_INFO_STREAM("adding new collision object to planning scene published!");
    return true;
  }
  else
  {
    ROS_ERROR_STREAM("Failed to publish planning scene diff srv!");
    return false;
  }
}

// TODO: temp input param
moveit_msgs::AttachedCollisionObject choreo_process_planning::addFullEndEffectorCollisionObject(bool is_add)
{
  // millimeter to meter
  double scale = 0.001;
  Eigen::Vector3d scale_vector(scale, scale, scale);
  moveit_msgs::AttachedCollisionObject attached_full_eef;
  attached_full_eef.link_name = "eef_base_link";

  /* A default pose */
  geometry_msgs::Pose pose;
  pose.position.x = 0.0;
  pose.position.y = 0.0;
  pose.position.z = 0.0;
  pose.orientation.w= 0.0;
  pose.orientation.x= 0.0;
  pose.orientation.y= 0.0;
  pose.orientation.z= 0.0;

  /* Define the full eef mesh */
  shapes::Mesh* m = shapes::createMeshFromResource(
      "package://asw_end_effector/meshes/collision/asw_hotend_end_effector_hull_bulky.stl", scale_vector);
  shape_msgs::Mesh mesh;
  shapes::ShapeMsg mesh_msg;
  shapes::constructMsgFromShape(m, mesh_msg);
  mesh = boost::get<shape_msgs::Mesh>(mesh_msg);

  attached_full_eef.object.header.frame_id = "eef_tcp_frame";
  attached_full_eef.object.id = "full_eef";

  attached_full_eef.object.meshes.resize(1);
  attached_full_eef.object.mesh_poses.resize(1);

  attached_full_eef.object.meshes[0] = mesh;
  attached_full_eef.object.mesh_poses[0] = pose;

  if(is_add)
  {
    attached_full_eef.object.operation = moveit_msgs::CollisionObject::ADD;
  }
  else
  {
    attached_full_eef.object.operation = moveit_msgs::CollisionObject::REMOVE;
  }

  return attached_full_eef;
}

bool choreo_process_planning::clearAllCollisionObjects(ros::ServiceClient& planning_scene_diff_client)
{
  // fetch scene
  ros::NodeHandle nh;
  auto planning_scene_fetch_client = nh.serviceClient<moveit_msgs::GetPlanningScene>(GET_PLANNING_SCENE_SERVICE);

  if(!planning_scene_fetch_client.waitForExistence())
  {
    ROS_ERROR_STREAM("[Process Planning] cannot connect with get fetch planning scene server...");
  }

  moveit_msgs::GetPlanningScene fetch_srv;
  fetch_srv.request.components.components = moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY;

  if(!planning_scene_fetch_client.call(fetch_srv))
  {
    ROS_ERROR_STREAM("[Process Planning] Failed to fetch planning scene srv!");
  }

  // publish diff scene
  if(!planning_scene_diff_client.waitForExistence())
  {
    ROS_ERROR_STREAM("cannot connect with planning scene diff server...");
  }

  moveit_msgs::ApplyPlanningScene srv;

  moveit_msgs::PlanningScene planning_scene_msg;
  for(auto& existing_obj : fetch_srv.response.scene.world.collision_objects)
  {
    existing_obj.operation = moveit_msgs::CollisionObject::REMOVE;
    planning_scene_msg.world.collision_objects.push_back(existing_obj);

//    ROS_INFO_STREAM("collision object # " << existing_obj.id << " removed.");
  }

  planning_scene_msg.is_diff = true;
  srv.request.scene = planning_scene_msg;

  if(planning_scene_diff_client.call(srv))
  {
//    ROS_INFO_STREAM("adding new collision object to planning scene published!");
    return true;
  }
  else
  {
    ROS_ERROR_STREAM("Failed to publish <Clear> planning scene diff srv!");
    return false;
  }
}

choreo_process_planning::DescartesTraj
choreo_process_planning::createJointPath(const std::vector<double>& start,
                                           const std::vector<double>& stop, double dtheta)
{
  JointVector path = interpolateJoint(start, stop, dtheta);
  DescartesTraj result;
  for (std::size_t i = 0; i < path.size(); ++i)
  {
    result.push_back(boost::make_shared<descartes_trajectory::JointTrajectoryPt>(path[i]));
  }
  return result;
}

trajectory_msgs::JointTrajectory choreo_process_planning::getMoveitPlan(
    const std::string& group_name, const std::vector<double>& joints_start,
    const std::vector<double>& joints_stop, moveit::core::RobotModelConstPtr model)
{
  const moveit::core::JointModelGroup* group = model->getJointModelGroup(group_name);
  robot_state::RobotState goal_state(model);
  goal_state.setJointGroupPositions(group, joints_stop);

  moveit_msgs::GetMotionPlan::Request req;
  req.motion_plan_request.group_name = group_name;
  req.motion_plan_request.num_planning_attempts = DEFAULT_MOVEIT_NUM_PLANNING_ATTEMPTS;
  req.motion_plan_request.max_velocity_scaling_factor = DEFAULT_MOVEIT_VELOCITY_SCALING;
  req.motion_plan_request.allowed_planning_time = DEFAULT_MOVEIT_PLANNING_TIME;
  req.motion_plan_request.planner_id = DEFAULT_MOVEIT_PLANNER_ID;

  req.motion_plan_request.workspace_parameters.header.frame_id = model->getRootLinkName();
  req.motion_plan_request.workspace_parameters.header.stamp = ros::Time::now();

  // Set the start state
  // Will want to add options here to start from a state that's not the start state
  moveit_msgs::RobotState start_state;
  start_state.is_diff = false;
  sensor_msgs::JointState joint_state;
  joint_state.name = group->getActiveJointModelNames();
  joint_state.position = joints_start;
  start_state.joint_state = joint_state;
  req.motion_plan_request.start_state = start_state;

  // Set the goal state
  moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints(goal_state, group);
  req.motion_plan_request.goal_constraints.push_back(c);

  // Make connection the planning-service offered by the MoveIt MoveGroup node
  ros::NodeHandle nh;

  std::string planner_name;
  if(!ros::param::get("~planner", planner_name))
  {
    ROS_WARN_STREAM("[Tr planning] planner param fetch fails.");
  }

  if("ompl" != planner_name)
  {
    ROS_INFO_STREAM("[Moveit Transition Planning] planner: " << planner_name);
  }
  else
  {
    ROS_INFO_STREAM("[Moveit Transition Planning] planner: " << planner_name << " - " << DEFAULT_MOVEIT_PLANNER_ID);
  }

  ros::ServiceClient client =
      nh.serviceClient<moveit_msgs::GetMotionPlan>(DEFAULT_MOVEIT_PLANNING_SERVICE_NAME);

  trajectory_msgs::JointTrajectory jt;
  moveit_msgs::GetMotionPlan::Response res;
  if (client.call(req, res))
  {
    jt = res.motion_plan_response.trajectory.joint_trajectory;
  }
  else
  {
    ROS_ERROR("%s: Unable to call MoveIt path planning service: '%s' or planning failed",
              __FUNCTION__, DEFAULT_MOVEIT_PLANNING_SERVICE_NAME.c_str());
//    throw std::runtime_error("Unable to generate MoveIt path plan");
  }
  return jt;
}

trajectory_msgs::JointTrajectory choreo_process_planning::getMoveitTransitionPlan(
    const std::string& group_name,
    const std::vector<double>& joints_start,
    const std::vector<double>& joints_stop,
    const std::vector<double>& initial_pose,
    moveit::core::RobotModelConstPtr model,
    const bool force_insert_reset)
{
  ros::NodeHandle nh;

  std::string planner_name;
  if(!ros::param::get("~planner", planner_name))
  {
    ROS_WARN_STREAM("[Tr planning] planner param fetch fails.");
  }

  const moveit::core::JointModelGroup* group = model->getJointModelGroup(group_name);
  robot_state::RobotState goal_state(model);
  goal_state.setJointGroupPositions(group, joints_stop);

  moveit_msgs::GetMotionPlan::Request req;
  req.motion_plan_request.group_name = group_name;
  req.motion_plan_request.num_planning_attempts = DEFAULT_MOVEIT_NUM_PLANNING_ATTEMPTS;
  req.motion_plan_request.max_velocity_scaling_factor = DEFAULT_MOVEIT_VELOCITY_SCALING;
  req.motion_plan_request.allowed_planning_time = DEFAULT_MOVEIT_PLANNING_TIME;

  if("ompl" == planner_name)
  {
    req.motion_plan_request.planner_id = DEFAULT_MOVEIT_PLANNER_ID;
  }

  req.motion_plan_request.workspace_parameters.header.frame_id = model->getRootLinkName();
  req.motion_plan_request.workspace_parameters.header.stamp = ros::Time::now();

  // Set the start state
  // Will want to add options here to start from a state that's not the start state
  moveit_msgs::RobotState start_state;
  start_state.is_diff = false;
  sensor_msgs::JointState joint_state;
  joint_state.name = group->getActiveJointModelNames();
  joint_state.position = joints_start;
  start_state.joint_state = joint_state;
  req.motion_plan_request.start_state = start_state;

  // Set the goal state
  moveit_msgs::Constraints c_goal = kinematic_constraints::constructGoalConstraints(goal_state, group);
  req.motion_plan_request.goal_constraints.push_back(c_goal);

  // Make connection the planning-service offered by the MoveIt MoveGroup node
  ros::ServiceClient client =
      nh.serviceClient<moveit_msgs::GetMotionPlan>(DEFAULT_MOVEIT_PLANNING_SERVICE_NAME);

  trajectory_msgs::JointTrajectory jt;
  moveit_msgs::GetMotionPlan::Response res;

  if("ompl" != planner_name)
  {
    ROS_INFO_STREAM("[Moveit Transition Planning] planner: " << planner_name);
  }
  else
  {
    ROS_INFO_STREAM("[Moveit Transition Planning] planner: " << planner_name << " - " << DEFAULT_MOVEIT_PLANNER_ID);
  }

  bool insert_reset = false;

  if(!force_insert_reset)
  {
    bool plan_call_success = client.call(req, res);
    if (plan_call_success && res.motion_plan_response.error_code.SUCCESS == res.motion_plan_response.error_code.val)
    {
      ROS_INFO_STREAM("[Tr Planning] direct transition planning succeed!");
      jt = res.motion_plan_response.trajectory.joint_trajectory;

      if ("ompl" == planner_name)
      {
        if (jt.points.size() > RRT_WEIRD_SOL_NUM_THRESHOLD)
        {
          ROS_WARN_STREAM("[Tr Planning] transition planning jt num overflow:" << jt.points.size()
                                                                               << ", current threshold: "
                                                                               << RRT_WEIRD_SOL_NUM_THRESHOLD
                                                                               << ". reset transition is forced.");
          insert_reset = true;
        }
      }
    }
    else
    {
      ROS_WARN_STREAM("[Tr Planning] direct transition planning failed.");
      insert_reset = true;
    }
  }

  if(force_insert_reset || insert_reset)
  {
    ROS_WARN("[Tr Planning] try resetting robot to intial pose and replan");

    moveit_msgs::GetMotionPlan::Request req_to_reset = req;
    moveit_msgs::GetMotionPlan::Request req_to_goal = req;

    // configure request to reset
    robot_state::RobotState reset_goal_state(model);
    reset_goal_state.setJointGroupPositions(group, initial_pose);
    moveit_msgs::Constraints c_reset = kinematic_constraints::constructGoalConstraints(reset_goal_state, group);
    req_to_reset.motion_plan_request.goal_constraints.clear();
    req_to_reset.motion_plan_request.goal_constraints.push_back(c_reset);

    // configure request to goal
    moveit_msgs::RobotState start_state_to_goal;
    joint_state.position = initial_pose;
    start_state_to_goal.is_diff = false;
    start_state_to_goal.joint_state = joint_state;
    req_to_goal.motion_plan_request.start_state = start_state_to_goal;

    req_to_goal.motion_plan_request.goal_constraints.clear();
    req_to_goal.motion_plan_request.goal_constraints.push_back(c_goal);

    // obtain trajectory
    trajectory_msgs::JointTrajectory jt_to_reset;
    trajectory_msgs::JointTrajectory jt_to_goal;

    // reset planning
    bool plan_call_success = client.call(req_to_reset, res);
    if (plan_call_success && res.motion_plan_response.error_code.SUCCESS == res.motion_plan_response.error_code.val)
    {
      jt_to_reset = res.motion_plan_response.trajectory.joint_trajectory;
      ROS_WARN("[Tr Planning] reset planning success.");
    }
    else
    {
      ROS_ERROR("%s: Unable to call MoveIt path planning service: '%s' or planning failed, AFTER RESETTING POSE",
                __FUNCTION__, DEFAULT_MOVEIT_PLANNING_SERVICE_NAME.c_str());
//      throw std::runtime_error("Unable to generate reset MoveIt path plan");
      jt.points.clear();

      return jt;
    }

    // goal planning
    plan_call_success = client.call(req_to_goal, res);
    if (plan_call_success && res.motion_plan_response.error_code.SUCCESS == res.motion_plan_response.error_code.val)
    {
      jt_to_goal = res.motion_plan_response.trajectory.joint_trajectory;
      ROS_WARN("[Tr Planning] reset to goal planning success.");
    }
    else
    {
      ROS_ERROR("%s: Unable to call MoveIt path planning service: '%s' or planning failed, AFTER RESETTING POSE",
                __FUNCTION__, DEFAULT_MOVEIT_PLANNING_SERVICE_NAME.c_str());
//      throw std::runtime_error("Unable to generate reset to goal MoveIt path plan");
      jt.points.clear();

      return jt;
    }

    // concatenate trajectories
    jt = jt_to_reset;
    ros::Duration last_time = jt.points.back().time_from_start;

    for(auto pt : jt_to_goal.points)
    {
      pt.time_from_start += last_time;
      jt.points.push_back(pt);
    }
  }
  return jt;
}

trajectory_msgs::JointTrajectory choreo_process_planning::planFreeMove(
    descartes_core::RobotModel& model, const std::string& group_name,
    moveit::core::RobotModelConstPtr moveit_model, const std::vector<double>& start,
    const std::vector<double>& stop)
{
  // Attempt joint interpolated motion
  DescartesTraj joint_approach = createJointPath(start, stop);

  // Check approach for collisions
  bool collision_free = true;
  for (std::size_t i = 0; i < joint_approach.size(); ++i)
  {
    if (!joint_approach[i]->isValid(model))
    {
      collision_free = false;
      break;
    }
  }

  // If the method is collision free, then we use the interpolation
  // otherwise let moveit try
  if (collision_free)
  {
    ROS_INFO_STREAM("[PlanFreeMove] Intepolated Plan used.");
    return toROSTrajectory(joint_approach, model);
  }
  else
  {
    ROS_INFO_STREAM("[PlanFreeMove] Moveit Plan used");
    return choreo_process_planning::getMoveitPlan(group_name, start, stop, moveit_model);
  }
}

double choreo_process_planning::freeSpaceCostFunction(const std::vector<double> &source,
                                                        const std::vector<double> &target)
{
  const double FREE_SPACE_MAX_ANGLE_DELTA =
      M_PI; // The maximum angle a joint during a freespace motion
  // from the start to end position without that motion
  // being penalized. Avoids flips.
  const double FREE_SPACE_ANGLE_PENALTY =
      2.0; // The factor by which a joint motion is multiplied if said
  // motion is greater than the max.

  // The cost function here penalizes large single joint motions in an effort to
  // keep the robot from flipping a joint, even if some other joints have to move
  // a bit more.
  double cost = 0.0;
  for (std::size_t i = 0; i < source.size(); ++i)
  {
    double diff = std::abs(source[i] - target[i]);
    if (diff > FREE_SPACE_MAX_ANGLE_DELTA)
      cost += FREE_SPACE_ANGLE_PENALTY * diff;
    else
      cost += diff;
  }
  return cost;
}