cart_trajectory_pt.cpp

Go to the documentation of this file.

/*
 * Software License Agreement (Apache License)
 *
 * Copyright (c) 2014, Dan Solomon
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
/*
 * cart_trajectory_pt.cpp
 *
 *  Created on: Oct 3, 2014
 *      Author: Dan Solomon
 */

#include <tuple>
#include <map>
#include <algorithm>
#include <console_bridge/console.h>
#include <ros/console.h>
#include <boost/uuid/uuid_io.hpp>
#include "descartes_trajectory/cart_trajectory_pt.h"
#include <descartes_core/utils.h>

#define NOT_IMPLEMENTED_ERR(ret) logError("%s not implemented", __PRETTY_FUNCTION__); return ret;

const double EQUALITY_TOLERANCE = 0.0001f;

using namespace descartes_core;

namespace descartes_trajectory
{
 EigenSTL::vector_Affine3d uniform(const TolerancedFrame & frame, const double orient_increment,
                                  const double pos_increment)
{

   EigenSTL::vector_Affine3d rtn;

   if(pos_increment < 0.0 || orient_increment < 0.0)
   {
     ROS_WARN_STREAM("Negative position/orientation intcrement: " << pos_increment
                     << "/" << orient_increment);
     rtn.clear();
     return rtn;
   }

  Eigen::Affine3d sampled_frame;

  // Calculating the number of samples for each tolerance (position and orientation)
  size_t ntx, nty, ntz, nrx, nry, nrz;

  if(orient_increment > 0)
  {
    nrx = ((frame.orientation_tolerance.x_upper - frame.orientation_tolerance.x_lower)/orient_increment) + 1;
    nry = ((frame.orientation_tolerance.y_upper - frame.orientation_tolerance.y_lower)/orient_increment) + 1;
    nrz = ((frame.orientation_tolerance.z_upper - frame.orientation_tolerance.z_lower)/orient_increment) + 1;
  }
  else
  {
    nrx = nry = nrz = 1;
  }

  if(pos_increment > 0)
  {
    ntx = ((frame.position_tolerance.x_upper - frame.position_tolerance.x_lower)/pos_increment) + 1;
    nty = ((frame.position_tolerance.y_upper - frame.position_tolerance.y_lower)/pos_increment) + 1;
    ntz = ((frame.position_tolerance.z_upper - frame.position_tolerance.z_lower)/pos_increment) + 1;
  }
  else
  {
    ntx = nty = ntz = 1;
  }

  // Estimating the number of samples base on tolerance zones and sampling increment.
  size_t est_num_samples = ntx * nty * ntz * nrx * nry * nrz;

  ROS_DEBUG_STREAM("Estimated number of samples: " << est_num_samples << ", reserving space");
  rtn.reserve(est_num_samples);

  //TODO: The following for loops do not ensure that the rull range is sample (lower to upper)
  //since there could be round off error in the incrementing of samples.  As a result, the
  //exact upper bound may not be sampled.  Since this isn't a final implementation, this will
  //be ignored.
  double rx, ry, rz, tx, ty, tz;

  for(size_t ii = 0; ii < nrx; ++ii)
  {
    rx = frame.orientation_tolerance.x_lower + orient_increment * ii;
    for(size_t jj = 0; jj < nry; ++jj)
    {
      ry = frame.orientation_tolerance.y_lower + orient_increment * jj;
      for(size_t kk = 0; kk < nrz; ++kk)
      {
        rz = frame.orientation_tolerance.z_lower + orient_increment * kk;
        for(size_t ll = 0; ll < ntx; ++ll)
        {
          tx = frame.position_tolerance.x_lower + pos_increment * ll;
          for(size_t mm = 0; mm < nty; ++mm)
          {
            ty = frame.position_tolerance.y_lower + pos_increment * mm;
            for(size_t nn = 0; nn < ntz; ++nn)
            {
              tz = frame.position_tolerance.z_lower + pos_increment * nn;

/*              sampled_frame = Eigen::Translation3d(tx,ty,tz) *
                  Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()) *
                  Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()) *
                  Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ());*/
              sampled_frame = descartes_core::utils::toFrame(tx,ty, tz,
                                             rx, ry, rz,descartes_core::utils::EulerConventions::XYZ);
              rtn.push_back(sampled_frame);
            }
          }
        }
      }
    }
  }
  ROS_DEBUG_STREAM("Uniform sampling of frame, utilizing orientation increment: " << orient_increment
                   << ", and position increment: " << pos_increment
                   << " resulted in " << rtn.size() << " samples");
  return rtn;
}


double distance(const std::vector<double>& j1, const std::vector<double>& j2)
{
 double rt = 0;
 double d;
 if(j1.size() == j2.size())
 {
   for(int i = 0 ; i < j1.size(); i++)
   {
     d = j1[i] - j2[i];
     rt += d*d;
   }
 }
 else
 {
   ROS_WARN_STREAM("Unequal size vectors, returning negative distance");
   return -1;
 }

 return std::sqrt(rt);
}

CartTrajectoryPt::CartTrajectoryPt(const descartes_core::TimingConstraint& timing)
  : descartes_core::TrajectoryPt(timing),
  tool_base_(Eigen::Affine3d::Identity()),
  tool_pt_(Eigen::Affine3d::Identity()),
  wobj_base_(Eigen::Affine3d::Identity()),
  wobj_pt_(Eigen::Affine3d::Identity()),
  pos_increment_(0.0),
  orient_increment_(0.0)
{}

CartTrajectoryPt::CartTrajectoryPt(const Frame &wobj_base, const TolerancedFrame &wobj_pt, const Frame &tool,
                 const TolerancedFrame &tool_pt, double pos_increment, double orient_increment,
                 const descartes_core::TimingConstraint& timing)
  : descartes_core::TrajectoryPt(timing),
  tool_base_(tool),
  tool_pt_(tool_pt),
  wobj_base_(wobj_base),
  wobj_pt_(wobj_pt),
  pos_increment_(pos_increment),
  orient_increment_(orient_increment)
{}


CartTrajectoryPt::CartTrajectoryPt(const TolerancedFrame &wobj_pt, double pos_increment,
                                   double orient_increment, const descartes_core::TimingConstraint& timing)
  : descartes_core::TrajectoryPt(timing),
  tool_base_(Eigen::Affine3d::Identity()),
  tool_pt_(Eigen::Affine3d::Identity()),
  wobj_base_(Eigen::Affine3d::Identity()),
  wobj_pt_(wobj_pt),
  pos_increment_(pos_increment),
  orient_increment_(orient_increment)
{}


CartTrajectoryPt::CartTrajectoryPt(const Frame &wobj_pt, const descartes_core::TimingConstraint& timing)
: descartes_core::TrajectoryPt(timing),
tool_base_(Eigen::Affine3d::Identity()),
tool_pt_(Eigen::Affine3d::Identity()),
wobj_base_(Eigen::Affine3d::Identity()),
wobj_pt_(wobj_pt),
pos_increment_(0),
orient_increment_(0)
{}

bool CartTrajectoryPt::getClosestCartPose(const std::vector<double> &seed_state,
                                          const RobotModel &model, Eigen::Affine3d &pose) const
{
  NOT_IMPLEMENTED_ERR(false);
}

bool CartTrajectoryPt::getNominalCartPose(const std::vector<double> &seed_state,
                                          const RobotModel &model, Eigen::Affine3d &pose) const
{
  /* Simply return wobj_pt expressed in world */
  pose = wobj_base_.frame * wobj_pt_.frame;
  return true;  //TODO can this ever return false?
}

bool CartTrajectoryPt::computeCartesianPoses(EigenSTL::vector_Affine3d &poses) const
{
  EigenSTL::vector_Affine3d sampled_wobj_pts = uniform(wobj_pt_, orient_increment_,
                                                     pos_increment_);
  EigenSTL::vector_Affine3d sampled_tool_pts = uniform(tool_pt_, orient_increment_,
                                                       pos_increment_);

  poses.clear();
  poses.reserve(sampled_wobj_pts.size()*sampled_tool_pts.size());
  for(size_t wobj_pt = 0; wobj_pt < sampled_wobj_pts.size(); ++wobj_pt)
  {
    for(size_t tool_pt = 0; tool_pt < sampled_tool_pts.size(); ++tool_pt)
    {
      Eigen::Affine3d pose = wobj_base_.frame * sampled_wobj_pts[wobj_pt] *
          sampled_tool_pts[tool_pt].inverse() * tool_base_.frame_inv;

      poses.push_back(pose);
    }
  }

  return !poses.empty();
}

void CartTrajectoryPt::getCartesianPoses(const RobotModel &model, EigenSTL::vector_Affine3d &poses) const
{
  EigenSTL::vector_Affine3d all_poses;
  poses.clear();

  if(computeCartesianPoses(all_poses))
  {
    poses.reserve(all_poses.size());
    for(const auto& pose: all_poses)
    {
      if(model.isValid(pose))
      {
        poses.push_back(pose);
      }
    }
  }
  else
  {
    ROS_ERROR("Failed for find ANY cartesian poses");
  }

  if( poses.empty())
  {
    ROS_WARN("Failed for find VALID cartesian poses, returning");
  }
  else
  {
    ROS_DEBUG_STREAM("Get cartesian poses, sampled: " << all_poses.size()
                     << ", with " << poses.size() << " valid(returned) poses");
  }

}

bool CartTrajectoryPt::getClosestJointPose(const std::vector<double> &seed_state,
                                           const RobotModel &model,
                                           std::vector<double> &joint_pose) const
{
  Eigen::Affine3d nominal_pose,candidate_pose;

  if(!model.getFK(seed_state,candidate_pose))
  {
    ROS_ERROR_STREAM("FK failed for seed pose for closest joint pose");
    return false;
  }

  // getting pose values
  Eigen::Vector3d t = candidate_pose.translation();
  Eigen::Vector3d rpy = candidate_pose.rotation().eulerAngles(0,1,2);

  std::vector< std::tuple<double,double,double> > vals =
  {
   std::make_tuple(t(0),wobj_pt_.position_tolerance.x_lower,wobj_pt_.position_tolerance.x_upper),
   std::make_tuple(t(1),wobj_pt_.position_tolerance.y_lower,wobj_pt_.position_tolerance.y_upper),
   std::make_tuple(t(2),wobj_pt_.position_tolerance.z_lower,wobj_pt_.position_tolerance.z_upper),
   std::make_tuple(rpy(0),wobj_pt_.orientation_tolerance.x_lower,wobj_pt_.orientation_tolerance.x_upper),
   std::make_tuple(rpy(1),wobj_pt_.orientation_tolerance.y_lower,wobj_pt_.orientation_tolerance.y_upper),
   std::make_tuple(rpy(2),wobj_pt_.orientation_tolerance.z_lower,wobj_pt_.orientation_tolerance.z_upper)
  };

  std::vector<double> closest_pose_vals = {t(0),t(1),t(2),rpy(0),rpy(1),rpy(2)};
  bool solve_ik = false;
  for(int i = 0; i < vals.size();i++)
  {
    auto &lower = std::get<1>(vals[i]);
    auto &upper = std::get<2>(vals[i]);
    auto &v = std::get<0>(vals[i]);

    if( std::abs(upper -lower) > EQUALITY_TOLERANCE)
    {
      auto bounds = std::make_pair(lower,upper);
      if(std::minmax({lower,v,upper}) != bounds)
      {
        solve_ik =  true;
        closest_pose_vals[i] = v < lower ? lower : upper;
        ROS_DEBUG("Cartesian nominal [%i] exceeded bounds: [val: %f, lower: %f, upper: %f]",i,v,lower,upper);
      }
    }
    else
    {
      if(std::abs(v-lower) > EQUALITY_TOLERANCE)
      {
        solve_ik =  true;
        ROS_DEBUG("Cartesian nominals [%i] differ: [val: %f, lower: %f, upper: %f]",i,v,lower,upper);
        closest_pose_vals[i] = lower;
      }
    }

  }

  if(solve_ik)
  {

    Eigen::Affine3d closest_pose = descartes_core::utils::toFrame(closest_pose_vals[0],
                                                                 closest_pose_vals[1],
                                                                 closest_pose_vals[2],
                                                                 closest_pose_vals[3],
                                                                 closest_pose_vals[4],
                                                                 closest_pose_vals[5],
                                                                 descartes_core::utils::EulerConventions::XYZ);
    if(!model.getIK(closest_pose,seed_state,joint_pose))
    {
      ROS_WARN_STREAM("Ik failed on closest pose");

      std::vector<std::vector<double> > joint_poses;
      getJointPoses(model,joint_poses);
      if(joint_poses.size() > 0)
      {

        ROS_WARN_STREAM("Closest cartesian pose not found, returning closest to seed joint pose");

        double sd = std::numeric_limits<double>::max();
        double d;
        for(auto j: joint_poses)
        {
          d = distance(seed_state,j);
          if(sd > d)
          {
            sd = d;
            joint_pose = j;
          }
        }

      }
      else
      {
        ROS_ERROR_STREAM("getClosestJointPose failed, no valid joint poses for this point");
        return false;
      }

      return true;
    }
  }
  else
  {

    joint_pose.assign(seed_state.begin(),seed_state.end());
  }

  return true;
}

bool CartTrajectoryPt::getNominalJointPose(const std::vector<double> &seed_state,
                                           const RobotModel &model,
                                           std::vector<double> &joint_pose) const
{
  Eigen::Affine3d robot_pose = wobj_base_.frame * wobj_pt_.frame *
      tool_pt_.frame_inv * tool_base_.frame_inv;
  return model.getIK(robot_pose, seed_state, joint_pose);
}

void CartTrajectoryPt::getJointPoses(const RobotModel &model,
                                     std::vector<std::vector<double> > &joint_poses) const
{
  joint_poses.clear();

  EigenSTL::vector_Affine3d poses;
  if(computeCartesianPoses(poses))
  {
    poses.reserve(poses.size());
    for(const auto& pose: poses)
    {
      std::vector<std::vector<double> > local_joint_poses;
      if(model.getAllIK(pose, local_joint_poses))
      {
        joint_poses.insert(joint_poses.end(), local_joint_poses.begin(), local_joint_poses.end());
      }
    }
  }
  else
  {
    ROS_ERROR("Failed for find ANY cartesian poses");
  }

  if( joint_poses.empty())
  {
    ROS_WARN("Failed for find ANY joint poses, returning");
  }
  else
  {
    ROS_DEBUG_STREAM("Get joint poses, sampled: " << poses.size()
                     << ", with " << joint_poses.size() << " valid(returned) poses");
  }
}

bool CartTrajectoryPt::isValid(const RobotModel &model) const
{
  Eigen::Affine3d robot_pose = wobj_base_.frame * wobj_pt_.frame *
      tool_pt_.frame_inv * tool_base_.frame_inv;
  return model.isValid(robot_pose);
}


bool CartTrajectoryPt::setDiscretization(const std::vector<double> &discretization)
{
  NOT_IMPLEMENTED_ERR(false);
}

} /* namespace descartes_trajectory */