trajectory_pt.h

Go to the documentation of this file.

/*
 * Software License Agreement (Apache License)
 *
 * Copyright (c) 2014, Southwest Research Institute
 *
 * 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.
 */
/*
 * trajectory_pt.h
 *
 *  Created on: Jun 5, 2014
 *      Author: Dan Solomon
 */

#ifndef TRAJECTORY_PT_H_
#define TRAJECTORY_PT_H_

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <eigen_stl_containers/eigen_stl_vector_container.h>
#include <vector>

#include "descartes_core/robot_model.h"
#include "descartes_core/trajectory_id.h"
#include "descartes_core/trajectory_timing_constraint.h"

namespace descartes_core
{
struct Frame
{
  Frame(){};
  Frame(const Eigen::Affine3d &a) : frame(a), frame_inv(a.inverse()){};

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  Eigen::Affine3d frame;
  Eigen::Affine3d frame_inv;

  static const Frame Identity()
  {
    return Frame(Eigen::Affine3d::Identity());
  }
};

DESCARTES_CLASS_FORWARD(TrajectoryPt);
class TrajectoryPt
{
public:
  typedef TrajectoryID ID;

  TrajectoryPt(const TimingConstraint &timing) : id_(TrajectoryID::make_id()), timing_(timing)
  {
  }

  virtual ~TrajectoryPt()
  {
  }

  virtual bool getClosestCartPose(const std::vector<double> &seed_state, const RobotModel &kinematics,
                                  Eigen::Affine3d &pose) const = 0;

  virtual bool getNominalCartPose(const std::vector<double> &seed_state, const RobotModel &kinematics,
                                  Eigen::Affine3d &pose) const = 0;

  virtual void getCartesianPoses(const RobotModel &kinematics, EigenSTL::vector_Affine3d &poses) const = 0;
  virtual bool getClosestJointPose(const std::vector<double> &seed_state, const RobotModel &model,
                                   std::vector<double> &joint_pose) const = 0;

  virtual bool getNominalJointPose(const std::vector<double> &seed_state, const RobotModel &model,
                                   std::vector<double> &joint_pose) const = 0;

  virtual void getJointPoses(const RobotModel &model, std::vector<std::vector<double> > &joint_poses) const = 0;
  virtual bool isValid(const RobotModel &model) const = 0;

  virtual bool setDiscretization(const std::vector<double> &discretization) = 0;

  inline ID getID() const
  {
    return id_;
  }

  void setID(const ID &id)
  {
    id_ = id;
  }
  virtual TrajectoryPtPtr copy() const = 0;

  virtual TrajectoryPtPtr copyAndSetTiming(const TimingConstraint &tm) const
  {
    TrajectoryPtPtr cp = copy();
    cp->setTiming(tm);
    return cp;
  }

  virtual TrajectoryPtPtr clone() const
  {
    TrajectoryPtPtr cp = copy();
    cp->setID(TrajectoryID::make_id());
    return cp;
  }

  virtual TrajectoryPtPtr cloneAndSetTiming(const TimingConstraint &tm) const
  {
    TrajectoryPtPtr cp = clone();
    cp->setTiming(tm);
    return cp;
  }

  const TimingConstraint &getTiming() const
  {
    return timing_;
  }

  void setTiming(const TimingConstraint &timing)
  {
    timing_ = timing;
  }

protected:
  ID id_; 
  TimingConstraint timing_; 
};

} /* namespace descartes_core */

#endif /* TRAJECTORY_PT_H_ */