cart_trajectory_pt.h

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.h
 *
 *  Created on: Oct 3, 2014
 *      Author: Dan Solomon
 */

#ifndef CART_TRAJECTORY_PT_H_
#define CART_TRAJECTORY_PT_H_

#include <moveit/kinematic_constraints/kinematic_constraint.h>
#include "descartes_core/trajectory_pt.h"
#include "ros/console.h"

typedef boost::shared_ptr<kinematic_constraints::PositionConstraint> PositionConstraintPtr;
typedef boost::shared_ptr<kinematic_constraints::OrientationConstraint> OrientationConstraintPtr;

namespace descartes_trajectory
{
struct ToleranceBase
{
  template <typename T>
  static T createSymmetric(double x, double y, double z, double x_tol, double y_tol, double z_tol)
  {
    double x_lower = x - x_tol / 2.0;
    double x_upper = x + x_tol / 2.0;

    double y_lower = y - y_tol / 2.0;
    double y_upper = y + y_tol / 2.0;

    double z_lower = z - z_tol / 2.0;
    double z_upper = z + z_tol / 2.0;

    return (T(x_lower, x_upper, y_lower, y_upper, z_lower, z_upper));
  }

  template <typename T>
  static T createSymmetric(const double &x, const double &y, const double &z, const double &tol)
  {
    return (createSymmetric<T>(x, y, z, tol, tol, tol));
  }

  template <typename T>
  static T zeroTolerance(const double &x, const double &y, const double &z)
  {
    return (createSymmetric<T>(x, y, z, 0.0, 0.0, 0.0));
  }

  ToleranceBase() : x_upper(0.), y_upper(0.), z_upper(0.), x_lower(0.), y_lower(0.), z_lower(0.)
  {
  }

  ToleranceBase(double x_lower_lim, double x_upper_lim, double y_lower_lim, double y_upper_lim, double z_lower_lim,
                double z_upper_lim)
    : x_upper(x_upper_lim)
    , y_upper(y_upper_lim)
    , z_upper(z_upper_lim)
    , x_lower(x_lower_lim)
    , y_lower(y_lower_lim)
    , z_lower(z_lower_lim)
  {
    ROS_DEBUG_STREAM("Creating fully defined Tolerance(base type)");
    ROS_DEBUG_STREAM("Initializing x tolerance (lower/upper)" << x_lower << "/" << x_upper);
    ROS_DEBUG_STREAM("Initializing y tolerance (lower/upper)" << y_lower << "/" << y_upper);
    ROS_DEBUG_STREAM("Initializing z tolerance (lower/upper)" << z_lower << "/" << z_upper);
  }

  void clear()
  {
    x_upper = y_upper = z_upper = x_lower = y_lower = z_lower = 0.;
  }

  double x_upper, y_upper, z_upper, x_lower, y_lower, z_lower;
};

struct PositionTolerance : public ToleranceBase
{
  PositionTolerance() : ToleranceBase()
  {
  }

  PositionTolerance(double x_lower_lim, double x_upper_lim, double y_lower_lim, double y_upper_lim, double z_lower_lim,
                    double z_upper_lim)
    : ToleranceBase(x_lower_lim, x_upper_lim, y_lower_lim, y_upper_lim, z_lower_lim, z_upper_lim)
  {
    ROS_DEBUG_STREAM("Created fully defined Position Tolerance");
  }
};

struct OrientationTolerance : public ToleranceBase
{
  OrientationTolerance() : ToleranceBase()
  {
  }

  OrientationTolerance(double x_lower_lim, double x_upper_lim, double y_lower_lim, double y_upper_lim,
                       double z_lower_lim, double z_upper_lim)
    : ToleranceBase(x_lower_lim, x_upper_lim, y_lower_lim, y_upper_lim, z_lower_lim, z_upper_lim)
  {
    ROS_DEBUG_STREAM("Created fully defined Orientation Tolerance");
  }
};

struct TolerancedFrame : public descartes_core::Frame
{
  TolerancedFrame(){};
  TolerancedFrame(const Eigen::Affine3d &a) : descartes_core::Frame(a)
  {
    Eigen::Vector3d t = a.translation();
    Eigen::Matrix3d m = a.rotation();
    Eigen::Vector3d rxyz = m.eulerAngles(0, 1, 2);
    position_tolerance = ToleranceBase::createSymmetric<PositionTolerance>(t(0), t(1), t(2), 0);
    orientation_tolerance = ToleranceBase::createSymmetric<OrientationTolerance>(rxyz(0), rxyz(1), rxyz(2), 0);
  };
  TolerancedFrame(const descartes_core::Frame &a) : descartes_core::Frame(a)
  {
    Eigen::Vector3d t = a.frame.translation();
    Eigen::Matrix3d m = a.frame.rotation();
    Eigen::Vector3d rxyz = m.eulerAngles(0, 1, 2);
    position_tolerance = ToleranceBase::createSymmetric<PositionTolerance>(t(0), t(1), t(2), 0);
    orientation_tolerance = ToleranceBase::createSymmetric<OrientationTolerance>(rxyz(0), rxyz(1), rxyz(2), 0);
  };

  TolerancedFrame(const Eigen::Affine3d &a, const PositionTolerance &pos_tol, const OrientationTolerance &orient_tol)
    : Frame(a), position_tolerance(pos_tol), orientation_tolerance(orient_tol)
  {
  }

  PositionTolerance position_tolerance;
  OrientationTolerance orientation_tolerance;
  PositionConstraintPtr position_constraint;
  OrientationConstraintPtr orientation_constraint;
};

class CartTrajectoryPt : public descartes_core::TrajectoryPt
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

public:
  CartTrajectoryPt(const descartes_core::TimingConstraint &timing = descartes_core::TimingConstraint());

  CartTrajectoryPt(const descartes_core::Frame &wobj_base, const TolerancedFrame &wobj_pt,
                   const descartes_core::Frame &tool_base, const TolerancedFrame &tool_pt, double pos_increment,
                   double orient_increment,
                   const descartes_core::TimingConstraint &timing = descartes_core::TimingConstraint());

  CartTrajectoryPt(const TolerancedFrame &wobj_pt, double pos_increment, double orient_increment,
                   const descartes_core::TimingConstraint &timing = descartes_core::TimingConstraint());

  CartTrajectoryPt(const descartes_core::Frame &wobj_pt,
                   const descartes_core::TimingConstraint &timing = descartes_core::TimingConstraint());

  virtual ~CartTrajectoryPt(){};

  // TODO complete
  virtual bool getClosestCartPose(const std::vector<double> &seed_state, const descartes_core::RobotModel &model,
                                  Eigen::Affine3d &pose) const;

  // TODO complete
  virtual bool getNominalCartPose(const std::vector<double> &seed_state, const descartes_core::RobotModel &model,
                                  Eigen::Affine3d &pose) const;

  // TODO complete
  virtual void getCartesianPoses(const descartes_core::RobotModel &model, EigenSTL::vector_Affine3d &poses) const;
  // TODO complete
  virtual bool getClosestJointPose(const std::vector<double> &seed_state, const descartes_core::RobotModel &model,
                                   std::vector<double> &joint_pose) const;
  // TODO complete
  virtual bool getNominalJointPose(const std::vector<double> &seed_state, const descartes_core::RobotModel &model,
                                   std::vector<double> &joint_pose) const;

  // TODO complete
  virtual void getJointPoses(const descartes_core::RobotModel &model,
                             std::vector<std::vector<double> > &joint_poses) const;
  // TODO complete
  virtual bool isValid(const descartes_core::RobotModel &model) const;
  virtual bool setDiscretization(const std::vector<double> &discretization);

  virtual descartes_core::TrajectoryPtPtr copy() const
  {
    return descartes_core::TrajectoryPtPtr(new CartTrajectoryPt(*this));
  }

  inline void setTool(const descartes_core::Frame &base, const TolerancedFrame &pt)
  {
    tool_base_ = base;
    tool_pt_ = pt;
  }

  inline void setWobj(const descartes_core::Frame &base, const TolerancedFrame &pt)
  {
    wobj_base_ = base;
    wobj_pt_ = pt;
  }

protected:
  bool computeCartesianPoses(EigenSTL::vector_Affine3d &poses) const;

protected:
  descartes_core::Frame tool_base_; 
  TolerancedFrame tool_pt_;         
  descartes_core::Frame wobj_base_; 
  TolerancedFrame wobj_pt_;         
  double pos_increment_;    
  double orient_increment_; 
};

} /* namespace descartes_trajectory */

#endif /* CART_TRAJECTORY_PT_H_ */