chomp_trajectory.cpp
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 #include <ros/ros.h>
00038 #include <chomp_motion_planner/chomp_trajectory.h>
00039 #include <iostream>
00040 
00041 namespace chomp
00042 {
00043 ChompTrajectory::ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, double duration,
00044                                  double discretization, std::string group_name)
00045   : planning_group_name_(group_name)
00046   , num_points_((duration / discretization) + 1)
00047   , discretization_(discretization)
00048   , duration_(duration)
00049   , start_index_(1)
00050   , end_index_(num_points_ - 2)
00051 {
00052   const moveit::core::JointModelGroup* model_group = robot_model->getJointModelGroup(planning_group_name_);
00053   num_joints_ = model_group->getActiveJointModels().size();
00054   init();
00055 }
00056 
00057 ChompTrajectory::ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, int num_points,
00058                                  double discretization, std::string group_name)
00059   : planning_group_name_(group_name)
00060   , num_points_(num_points)
00061   , discretization_(discretization)
00062   , duration_((num_points - 1) * discretization)
00063   , start_index_(1)
00064   , end_index_(num_points_ - 2)
00065 {
00066   const moveit::core::JointModelGroup* model_group = robot_model->getJointModelGroup(planning_group_name_);
00067   num_joints_ = model_group->getActiveJointModels().size();
00068   init();
00069 }
00070 
00071 ChompTrajectory::ChompTrajectory(const ChompTrajectory& source_traj, const std::string& planning_group,
00072                                  int diff_rule_length)
00073   : planning_group_name_(planning_group), discretization_(source_traj.discretization_)
00074 {
00075   num_joints_ = source_traj.getNumJoints();
00076 
00077   // figure out the num_points_:
00078   // we need diff_rule_length-1 extra points on either side:
00079   int start_extra = (diff_rule_length - 1) - source_traj.start_index_;
00080   int end_extra = (diff_rule_length - 1) - ((source_traj.num_points_ - 1) - source_traj.end_index_);
00081 
00082   num_points_ = source_traj.num_points_ + start_extra + end_extra;
00083   start_index_ = diff_rule_length - 1;
00084   end_index_ = (num_points_ - 1) - (diff_rule_length - 1);
00085   duration_ = (num_points_ - 1) * discretization_;
00086 
00087   // allocate the memory:
00088   init();
00089 
00090   full_trajectory_index_.resize(num_points_);
00091 
00092   // now copy the trajectories over:
00093   for (int i = 0; i < num_points_; i++)
00094   {
00095     int source_traj_point = i - start_extra;
00096     if (source_traj_point < 0)
00097       source_traj_point = 0;
00098     if (source_traj_point >= source_traj.num_points_)
00099       source_traj_point = source_traj.num_points_ - 1;
00100     full_trajectory_index_[i] = source_traj_point;
00101     for (int j = 0; j < num_joints_; j++)
00102     {
00103       (*this)(i, j) = source_traj(source_traj_point, j);
00104     }
00105   }
00106 }
00107 
00108 ChompTrajectory::ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const std::string& planning_group,
00109                                  const trajectory_msgs::JointTrajectory& traj)
00110   : planning_group_name_(planning_group)
00111 {
00112   const moveit::core::JointModelGroup* model_group = robot_model->getJointModelGroup(planning_group_name_);
00113   num_joints_ = model_group->getActiveJointModels().size();
00114   double discretization = (traj.points[1].time_from_start - traj.points[0].time_from_start).toSec();
00115 
00116   double discretization2 = (traj.points[2].time_from_start - traj.points[1].time_from_start).toSec();
00117 
00118   if (fabs(discretization2 - discretization) > .001)
00119   {
00120     ROS_WARN_STREAM("Trajectory Discretization not constant " << discretization << " " << discretization2);
00121   }
00122   discretization_ = discretization;
00123 
00124   num_points_ = traj.points.size() + 1;
00125   duration_ = (traj.points.back().time_from_start - traj.points[0].time_from_start).toSec();
00126 
00127   start_index_ = 1;
00128   end_index_ = num_points_ - 2;
00129 
00130   init();
00131 
00132   for (int i = 0; i < num_points_; i++)
00133   {
00134     for (int j = 0; j < num_joints_; j++)
00135     {
00136       trajectory_(i, j) = 0.0;
00137     }
00138   }
00139   overwriteTrajectory(traj);
00140 }
00141 
00142 ChompTrajectory::~ChompTrajectory()
00143 {
00144 }
00145 
00146 void ChompTrajectory::overwriteTrajectory(const trajectory_msgs::JointTrajectory& traj)
00147 {
00148   for (unsigned int i = 1; i <= traj.points.size(); i++)
00149   {
00150     for (unsigned int j = 0; j < traj.joint_names.size(); j++)
00151     {
00152       trajectory_(i, j) = traj.points[i - 1].positions[j];
00153     }
00154   }
00155 }
00156 
00157 void ChompTrajectory::init()
00158 {
00159   trajectory_.resize(num_points_, num_joints_);
00160   trajectory_ = Eigen::MatrixXd(num_points_, num_joints_);
00161 }
00162 
00163 void ChompTrajectory::updateFromGroupTrajectory(const ChompTrajectory& group_trajectory)
00164 {
00165   int num_vars_free = end_index_ - start_index_ + 1;
00166   for (int i = 0; i < num_joints_; i++)
00167   {
00168     trajectory_.block(start_index_, i, num_vars_free, 1) =
00169         group_trajectory.trajectory_.block(group_trajectory.start_index_, i, num_vars_free, 1);
00170   }
00171 }
00172 
00173 void ChompTrajectory::fillInMinJerk()
00174 {
00175   double start_index = start_index_ - 1;
00176   double end_index = end_index_ + 1;
00177   double T[6];  // powers of the time duration
00178   T[0] = 1.0;
00179   T[1] = (end_index - start_index) * discretization_;
00180 
00181   for (int i = 2; i <= 5; i++)
00182     T[i] = T[i - 1] * T[1];
00183 
00184   // calculate the spline coefficients for each joint:
00185   // (these are for the special case of zero start and end vel and acc)
00186   double coeff[num_joints_][6];
00187   for (int i = 0; i < num_joints_; i++)
00188   {
00189     double x0 = (*this)(start_index, i);
00190     double x1 = (*this)(end_index, i);
00191     coeff[i][0] = x0;
00192     coeff[i][1] = 0;
00193     coeff[i][2] = 0;
00194     coeff[i][3] = (-20 * x0 + 20 * x1) / (2 * T[3]);
00195     coeff[i][4] = (30 * x0 - 30 * x1) / (2 * T[4]);
00196     coeff[i][5] = (-12 * x0 + 12 * x1) / (2 * T[5]);
00197   }
00198 
00199   // now fill in the joint positions at each time step
00200   for (int i = start_index + 1; i < end_index; i++)
00201   {
00202     double t[6];  // powers of the time index point
00203     t[0] = 1.0;
00204     t[1] = (i - start_index) * discretization_;
00205     for (int k = 2; k <= 5; k++)
00206       t[k] = t[k - 1] * t[1];
00207 
00208     for (int j = 0; j < num_joints_; j++)
00209     {
00210       (*this)(i, j) = 0.0;
00211       for (int k = 0; k <= 5; k++)
00212       {
00213         (*this)(i, j) += t[k] * coeff[j][k];
00214       }
00215     }
00216   }
00217 }
00218 
00219 }  // namespace chomp


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