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 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 
00035 /* Author: Mrinal Kalakrishnan */
00036 
00037 #ifndef CHOMP_TRAJECTORY_H_
00038 #define CHOMP_TRAJECTORY_H_
00039 
00040 #include <trajectory_msgs/JointTrajectory.h>
00041 #include <moveit/robot_model/robot_model.h>
00042 #include <chomp_motion_planner/chomp_utils.h>
00043 
00044 #include <vector>
00045 #include <eigen3/Eigen/Core>
00046 
00047 namespace chomp
00048 {
00052 class ChompTrajectory
00053 {
00054 public:
00058   ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, double duration, double discretization,
00059                   std::string groupName);
00060 
00064   ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, int num_points, double discretization,
00065                   std::string groupName);
00066 
00071   ChompTrajectory(const ChompTrajectory& source_traj, const std::string& planning_group, int diff_rule_length);
00072 
00073   ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, 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   Eigen::MatrixXd::ColXpr getJointTrajectory(int joint);
00088 
00089   void overwriteTrajectory(const trajectory_msgs::JointTrajectory& traj);
00090 
00094   int getNumPoints() const;
00095 
00099   int getNumFreePoints() const;
00100 
00104   int getNumJoints() const;
00105 
00109   double getDiscretization() const;
00110 
00116   void fillInMinJerk();
00117 
00124   void setStartEndIndex(int start_index, int end_index);
00125 
00129   int getStartIndex() const;
00130 
00134   int getEndIndex() const;
00135 
00139   Eigen::MatrixXd& getTrajectory();
00140 
00144   Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic> getFreeTrajectoryBlock();
00145 
00149   Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic> getFreeJointTrajectoryBlock(int joint);
00150 
00154   void updateFromGroupTrajectory(const ChompTrajectory& group_trajectory);
00155 
00159   int getFullTrajectoryIndex(int i) const;
00160 
00164   template <typename Derived>
00165   void getJointVelocities(int traj_point, Eigen::MatrixBase<Derived>& velocities);
00166 
00167   double getDuration() const;
00168 
00169 private:
00170   void init(); 
00172   std::string planning_group_name_; 
00173   int num_points_;                  
00174   int num_joints_;                  
00175   double discretization_;           
00176   double duration_;                 
00177   Eigen::MatrixXd trajectory_;      
00178   int start_index_; 
00180   int end_index_; 
00181   std::vector<int> full_trajectory_index_; 
00184 };
00185 
00187 
00188 inline double& ChompTrajectory::operator()(int traj_point, int joint)
00189 {
00190   return trajectory_(traj_point, joint);
00191 }
00192 
00193 inline double ChompTrajectory::operator()(int traj_point, int joint) const
00194 {
00195   return trajectory_(traj_point, joint);
00196 }
00197 
00198 inline Eigen::MatrixXd::RowXpr ChompTrajectory::getTrajectoryPoint(int traj_point)
00199 {
00200   return trajectory_.row(traj_point);
00201 }
00202 
00203 inline Eigen::MatrixXd::ColXpr ChompTrajectory::getJointTrajectory(int joint)
00204 {
00205   return trajectory_.col(joint);
00206 }
00207 
00208 inline int ChompTrajectory::getNumPoints() const
00209 {
00210   return num_points_;
00211 }
00212 
00213 inline int ChompTrajectory::getNumFreePoints() const
00214 {
00215   return (end_index_ - start_index_) + 1;
00216 }
00217 
00218 inline int ChompTrajectory::getNumJoints() const
00219 {
00220   return num_joints_;
00221 }
00222 
00223 inline double ChompTrajectory::getDiscretization() const
00224 {
00225   return discretization_;
00226 }
00227 
00228 inline void ChompTrajectory::setStartEndIndex(int start_index, int end_index)
00229 {
00230   start_index_ = start_index;
00231   end_index_ = end_index;
00232 }
00233 
00234 inline int ChompTrajectory::getStartIndex() const
00235 {
00236   return start_index_;
00237 }
00238 
00239 inline int ChompTrajectory::getEndIndex() const
00240 {
00241   return end_index_;
00242 }
00243 
00244 inline Eigen::MatrixXd& ChompTrajectory::getTrajectory()
00245 {
00246   return trajectory_;
00247 }
00248 
00249 inline Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic> ChompTrajectory::getFreeTrajectoryBlock()
00250 {
00251   return trajectory_.block(start_index_, 0, getNumFreePoints(), getNumJoints());
00252 }
00253 
00254 inline Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic>
00255 ChompTrajectory::getFreeJointTrajectoryBlock(int joint)
00256 {
00257   return trajectory_.block(start_index_, joint, getNumFreePoints(), 1);
00258 }
00259 
00260 inline int ChompTrajectory::getFullTrajectoryIndex(int i) const
00261 {
00262   return full_trajectory_index_[i];
00263 }
00264 
00265 template <typename Derived>
00266 void ChompTrajectory::getJointVelocities(int traj_point, Eigen::MatrixBase<Derived>& velocities)
00267 {
00268   velocities.setZero();
00269   double invTime = 1.0 / discretization_;
00270 
00271   for (int k = -DIFF_RULE_LENGTH / 2; k <= DIFF_RULE_LENGTH / 2; k++)
00272   {
00273     velocities += (invTime * DIFF_RULES[0][k + DIFF_RULE_LENGTH / 2]) * trajectory_.row(traj_point + k).transpose();
00274   }
00275 }
00276 
00277 inline double ChompTrajectory::getDuration() const
00278 {
00279   return duration_;
00280 }
00281 }
00282 
00283 #endif /* CHOMP_TRAJECTORY_H_ */


chomp_motion_planner
Author(s): Gil Jones , Mrinal Kalakrishnan
autogenerated on Wed Jun 19 2019 19:24:07