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

#ifndef JOINT_TRAJECTORY_PT_H_
#define JOINT_TRAJECTORY_PT_H_

#include <vector>
#include "descartes_core/trajectory_pt.h"

namespace descartes_trajectory
{

struct TolerancedJointValue
{
  TolerancedJointValue(double nominal, double lower, double upper)
    : nominal(nominal), lower(lower), upper(upper)
  {}

  TolerancedJointValue(double nominal)
    : nominal(nominal), lower(nominal), upper(nominal)
  {}

  double range() const
  {
    return upper - lower;
  }

  double nominal, lower, upper;
};

class JointTrajectoryPt: public descartes_core::TrajectoryPt
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;      //TODO is this needed when Frame already has it?
public:
  JointTrajectoryPt(const descartes_core::TimingConstraint& timing = descartes_core::TimingConstraint());

  JointTrajectoryPt(const std::vector<TolerancedJointValue> &joints, const descartes_core::Frame &tool, 
                    const descartes_core::Frame &wobj, 
                    const descartes_core::TimingConstraint& timing = descartes_core::TimingConstraint());


  JointTrajectoryPt(const std::vector<TolerancedJointValue> &joints, 
                    const descartes_core::TimingConstraint& timing = descartes_core::TimingConstraint());


  JointTrajectoryPt(const std::vector<double> &joints, 
                    const descartes_core::TimingConstraint& timing = descartes_core::TimingConstraint());

  virtual ~JointTrajectoryPt() {};

  //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;

  //TODO complete
  virtual bool setDiscretization(const std::vector<double> &discretization);

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

  void setJoints(const std::vector<TolerancedJointValue> &joints);

  inline
  void setTool(const descartes_core::Frame &tool)
  {
    tool_ = tool;
  }

  inline
  void setWobj(const descartes_core::Frame &wobj)
  {
    wobj_ = wobj;
  }
  inline
  const std::vector<double>& nominal() const
  {
    return nominal_;
  }

  inline
  const std::vector<double>& upper() const
  {
    return upper_;
  }

  inline
  const std::vector<double>& lower() const
  {
    return lower_;
  }

protected:
  std::vector<double> nominal_;
  std::vector<double> lower_;
  std::vector<double> upper_;
  std::vector<double> discretization_;  
  descartes_core::Frame                         tool_;                  
  descartes_core::Frame                         wobj_;                  
};

} /* namespace descartes_trajectory */



#endif /* JOINT_TRAJECTORY_PT_H_ */