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_ */