chomp_trajectory.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00037 #ifndef CHOMP_TRAJECTORY_H_
00038 #define CHOMP_TRAJECTORY_H_
00039 
00040 #include <trajectory_msgs/JointTrajectory.h>
00041 #include <planning_models/kinematic_model.h>
00042 #include <chomp_motion_planner/chomp_utils.h>
00043 
00044 #include <vector>
00045 #include <eigen3/Eigen/Core>
00046 
00047 namespace chomp
00048 {
00049 
00053 class ChompTrajectory
00054 {
00055 public:
00059   ChompTrajectory(const planning_models::KinematicModel* robot_model, double duration, double discretization, std::string groupName);
00060 
00064   ChompTrajectory(const planning_models::KinematicModel* robot_model, int num_points, double discretization, std::string groupName);
00065 
00070   ChompTrajectory(const ChompTrajectory& source_traj, const std::string& planning_group, int diff_rule_length);
00071 
00072   ChompTrajectory(const planning_models::KinematicModel* robot_model,
00073                   const std::string& planning_group,
00074                   const trajectory_msgs::JointTrajectory& traj);
00075 
00079   virtual ~ChompTrajectory();
00080 
00081   double& operator() (int traj_point, int joint);
00082 
00083   double operator() (int traj_point, int joint) const;
00084 
00085   Eigen::MatrixXd::RowXpr getTrajectoryPoint(int traj_point);
00086 
00087 
00088   Eigen::MatrixXd::ColXpr getJointTrajectory(int joint);
00089 
00090   void overwriteTrajectory(const trajectory_msgs::JointTrajectory& traj);
00091 
00095   int getNumPoints() const;
00096 
00100   int getNumFreePoints() const;
00101 
00105   int getNumJoints() const;
00106 
00110   double getDiscretization() const;
00111 
00117   void fillInMinJerk();
00118 
00125   void setStartEndIndex(int start_index, int end_index);
00126 
00130   int getStartIndex() const;
00131 
00135   int getEndIndex() const;
00136 
00140   Eigen::MatrixXd& getTrajectory();
00141 
00145   Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic>  getFreeTrajectoryBlock();
00146 
00150   Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic> getFreeJointTrajectoryBlock(int joint);
00151 
00155   void updateFromGroupTrajectory(const ChompTrajectory& group_trajectory);
00156 
00160   int getFullTrajectoryIndex(int i) const;
00161 
00165   template <typename Derived>
00166   void getJointVelocities(int traj_point, Eigen::MatrixBase<Derived>& velocities);
00167 
00168   double getDuration() const;
00169 
00170 private:
00171 
00172   void init();                                          
00174   std::string planning_group_name_;    
00175   int num_points_;                                      
00176   int num_joints_;                                      
00177   double discretization_;                               
00178   double duration_;                                     
00179   Eigen::MatrixXd trajectory_;                          
00180   int start_index_;                                     
00181   int end_index_;                                       
00182   std::vector<int> full_trajectory_index_;              
00183 };
00184 
00186 
00187 inline double& ChompTrajectory::operator() (int traj_point, int joint)
00188 {
00189   return trajectory_(traj_point, joint);
00190 }
00191 
00192 inline double ChompTrajectory::operator() (int traj_point, int joint) const
00193 {
00194   return trajectory_(traj_point, joint);
00195 }
00196 
00197 inline Eigen::MatrixXd::RowXpr ChompTrajectory::getTrajectoryPoint(int traj_point)
00198 {
00199   return trajectory_.row(traj_point);
00200 }
00201 
00202 inline Eigen::MatrixXd::ColXpr ChompTrajectory::getJointTrajectory(int joint)
00203 {
00204   return trajectory_.col(joint);
00205 }
00206 
00207 inline int ChompTrajectory::getNumPoints() const
00208 {
00209   return num_points_;
00210 }
00211 
00212 inline int ChompTrajectory::getNumFreePoints() const
00213 {
00214   return (end_index_ - start_index_)+1;
00215 }
00216 
00217 inline int ChompTrajectory::getNumJoints() const
00218 {
00219   return num_joints_;
00220 }
00221 
00222 inline double ChompTrajectory::getDiscretization() const
00223 {
00224   return discretization_;
00225 }
00226 
00227 inline void ChompTrajectory::setStartEndIndex(int start_index, int end_index)
00228 {
00229   start_index_ = start_index;
00230   end_index_ = end_index;
00231 }
00232 
00233 inline int ChompTrajectory::getStartIndex() const
00234 {
00235   return start_index_;
00236 }
00237 
00238 inline int ChompTrajectory::getEndIndex() const
00239 {
00240   return end_index_;
00241 }
00242 
00243 inline Eigen::MatrixXd& ChompTrajectory::getTrajectory()
00244 {
00245   return trajectory_;
00246 }
00247 
00248 inline Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic> ChompTrajectory::getFreeTrajectoryBlock()
00249 {
00250   return trajectory_.block(start_index_, 0, getNumFreePoints(), getNumJoints());
00251 }
00252 
00253 inline Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic> ChompTrajectory::getFreeJointTrajectoryBlock(int joint)
00254 {
00255   return trajectory_.block(start_index_, joint, getNumFreePoints(), 1);
00256 }
00257 
00258 inline int ChompTrajectory::getFullTrajectoryIndex(int i) const
00259 {
00260   return full_trajectory_index_[i];
00261 }
00262 
00263 template <typename Derived>
00264 void ChompTrajectory::getJointVelocities(int traj_point, Eigen::MatrixBase<Derived>& velocities)
00265 {
00266   velocities.setZero();
00267   double invTime = 1.0 / discretization_;
00268 
00269   for (int k=-DIFF_RULE_LENGTH/2; k<=DIFF_RULE_LENGTH/2; k++)
00270   {
00271     velocities += (invTime * DIFF_RULES[0][k+DIFF_RULE_LENGTH/2]) * trajectory_.row(traj_point+k).transpose();
00272   }
00273 }
00274 
00275 inline double ChompTrajectory::getDuration() const {
00276   return duration_;
00277 }
00278 
00279 
00280 }
00281 
00282 #endif /* CHOMP_TRAJECTORY_H_ */


chomp_motion_planner
Author(s): Mrinal Kalakrishnan / mail@mrinal.net
autogenerated on Thu Dec 12 2013 11:08:58