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