control_cost_projection.cpp
Go to the documentation of this file.
00001 
00028 #include <stomp_moveit/update_filters/control_cost_projection.h>
00029 #include <ros/console.h>
00030 #include <pluginlib/class_list_macros.h>
00031 #include <stomp_core/utils.h>
00032 
00033 PLUGINLIB_EXPORT_CLASS(stomp_moveit::update_filters::ControlCostProjection,stomp_moveit::update_filters::StompUpdateFilter);
00034 
00035 
00036 namespace stomp_moveit
00037 {
00038 namespace update_filters
00039 {
00040 
00041 double DEFAULT_TIME_STEP = 1.0;
00042 
00043 
00044 ControlCostProjection::ControlCostProjection():
00045     name_("ControlCostProjectionMatrix"),
00046     num_timesteps_(0)
00047 
00048 {
00049 
00050 }
00051 
00052 ControlCostProjection::~ControlCostProjection()
00053 {
00054   // TODO Auto-generated destructor stub
00055 }
00056 
00057 bool ControlCostProjection::initialize(moveit::core::RobotModelConstPtr robot_model_ptr,
00058                         const std::string& group_name,const XmlRpc::XmlRpcValue& config)
00059 {
00060   robot_model_ = robot_model_ptr;
00061   group_name_ = group_name;
00062 
00063   if(!configure(config))
00064   {
00065     return false;
00066   }
00067 
00068   return true;
00069 }
00070 
00071 bool ControlCostProjection::configure(const XmlRpc::XmlRpcValue& config)
00072 {
00073   return true;
00074 }
00075 
00076 bool ControlCostProjection::setMotionPlanRequest(const planning_scene::PlanningSceneConstPtr& planning_scene,
00077                  const moveit_msgs::MotionPlanRequest &req,
00078                  const stomp_core::StompConfiguration &config,
00079                  moveit_msgs::MoveItErrorCodes& error_code)
00080 {
00081 
00082   num_timesteps_ = config.num_timesteps;
00083   stomp_core::generateSmoothingMatrix(num_timesteps_,DEFAULT_TIME_STEP,projection_matrix_M_);
00084 
00085   // zeroing out first and last rows
00086   projection_matrix_M_.topRows(1) = Eigen::VectorXd::Zero(num_timesteps_).transpose();
00087   projection_matrix_M_(0,0) = 1.0;
00088   projection_matrix_M_.bottomRows(1) = Eigen::VectorXd::Zero(num_timesteps_).transpose();
00089   projection_matrix_M_(num_timesteps_ -1 ,num_timesteps_ -1 ) = 1;
00090 
00091   error_code.val = error_code.SUCCESS;
00092   return true;
00093 }
00094 
00095 bool ControlCostProjection::filter(std::size_t start_timestep,std::size_t num_timesteps,int iteration_number,
00096                                    const Eigen::MatrixXd& parameters,
00097                                    Eigen::MatrixXd& updates,
00098                                    bool& filtered)
00099 {
00100 
00101   for(auto d = 0u; d < updates.rows();d++)
00102   {
00103     updates.row(d).transpose() = projection_matrix_M_ * (updates.row(d).transpose());
00104   }
00105 
00106   filtered = true;
00107 
00108   return true;
00109 }
00110 
00111 } /* namespace update_filters */
00112 } /* namespace stomp_moveit */


stomp_moveit
Author(s): Jorge Nicho
autogenerated on Sat Jun 8 2019 19:24:01