trajectory_pt.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (Apache License)
00003  *
00004  * Copyright (c) 2014, Southwest Research Institute
00005  *
00006  * Licensed under the Apache License, Version 2.0 (the "License");
00007  * you may not use this file except in compliance with the License.
00008  * You may obtain a copy of the License at
00009  *
00010  * http://www.apache.org/licenses/LICENSE-2.0
00011  *
00012  * Unless required by applicable law or agreed to in writing, software
00013  * distributed under the License is distributed on an "AS IS" BASIS,
00014  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00015  * See the License for the specific language governing permissions and
00016  * limitations under the License.
00017  */
00018 /*
00019  * trajectory_pt.h
00020  *
00021  *  Created on: Jun 5, 2014
00022  *      Author: Dan Solomon
00023  */
00024 
00025 #ifndef TRAJECTORY_PT_H_
00026 #define TRAJECTORY_PT_H_
00027 
00028 #include <Eigen/Core>
00029 #include <Eigen/Geometry>
00030 #include <eigen_stl_containers/eigen_stl_vector_container.h>
00031 #include <vector>
00032 
00033 #include "descartes_core/robot_model.h"
00034 #include "descartes_core/trajectory_id.h"
00035 #include "descartes_core/trajectory_timing_constraint.h"
00036 
00037 namespace descartes_core
00038 {
00039 
00043 struct Frame
00044 {
00045   Frame(){};
00046   Frame(const Eigen::Affine3d &a):
00047     frame(a), frame_inv(a.inverse()) {};
00048 
00049   EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
00050   Eigen::Affine3d frame;
00051   Eigen::Affine3d frame_inv;
00052 
00053   static const Frame Identity()
00054   {
00055     return Frame(Eigen::Affine3d::Identity());
00056   }
00057 };
00058 
00059 
00069 DESCARTES_CLASS_FORWARD(TrajectoryPt);
00070 class TrajectoryPt
00071 {
00072 public:
00073   typedef TrajectoryID ID;
00074 
00075   TrajectoryPt(const TimingConstraint& timing) 
00076     : id_(TrajectoryID::make_id())
00077     , timing_(timing)
00078   {}
00079   
00080   virtual ~TrajectoryPt() {}
00081 
00094   virtual bool getClosestCartPose(const std::vector<double> &seed_state,
00095                                   const RobotModel &kinematics, Eigen::Affine3d &pose) const = 0;
00096 
00104   virtual bool getNominalCartPose(const std::vector<double> &seed_state,
00105                                   const RobotModel &kinematics, Eigen::Affine3d &pose) const = 0;
00106 
00111   virtual void getCartesianPoses(const RobotModel &kinematics, EigenSTL::vector_Affine3d &poses) const = 0;
00125   virtual bool getClosestJointPose(const std::vector<double> &seed_state,
00126                                    const RobotModel &model,
00127                                    std::vector<double> &joint_pose) const = 0;
00128 
00135   virtual bool getNominalJointPose(const std::vector<double> &seed_state,
00136                                    const RobotModel &model,
00137                                    std::vector<double> &joint_pose) const = 0;
00138 
00143   virtual void getJointPoses(const RobotModel &model,
00144                              std::vector<std::vector<double> > &joint_poses) const = 0;
00150   virtual bool isValid(const RobotModel &model) const = 0;
00151 
00156   virtual bool setDiscretization(const std::vector<double> &discretization) = 0;
00157 
00162   inline
00163   ID getID() const
00164   {
00165     return id_;
00166   }
00167 
00171   void setID(const ID &id)
00172   {
00173     id_ = id;
00174   }
00181   virtual TrajectoryPtPtr copy() const = 0;
00182 
00187   virtual TrajectoryPtPtr clone() const
00188   {
00189     TrajectoryPtPtr cp = copy();
00190     cp->setID(TrajectoryID::make_id());
00191     return cp;
00192   }
00193 
00194   const TimingConstraint& getTiming() const
00195   {
00196     return timing_;
00197   }
00198 
00199   void setTiming(const TimingConstraint& timing)
00200   {
00201     timing_ = timing;
00202   }
00203 
00204 protected:
00205   ID                    id_;      
00206   TimingConstraint      timing_;  
00208 };
00209 
00210 } /* namespace descartes_core */
00211 
00212 
00213 #endif /* TRAJECTORY_PT_H_ */


descartes_core
Author(s): Dan Solomon
autogenerated on Wed Aug 26 2015 11:21:21