simpson_integrator.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *   http://www.apache.org/licenses/LICENSE-2.0
00009 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 
00018 #ifndef COB_TWIST_CONTROLLER_UTILS_SIMPSON_INTEGRATOR_H
00019 #define COB_TWIST_CONTROLLER_UTILS_SIMPSON_INTEGRATOR_H
00020 
00021 #include <vector>
00022 
00023 #include <ros/ros.h>
00024 #include <kdl/jntarray.hpp>
00025 #include <std_msgs/Float64MultiArray.h>
00026 
00027 #include "cob_twist_controller/utils/moving_average.h"
00028 
00029 class SimpsonIntegrator
00030 {
00031     public:
00032         explicit SimpsonIntegrator(const uint8_t dof, const double integrator_smoothing = 0.2)
00033             : dof_(dof),
00034               integrator_smoothing_(integrator_smoothing),
00035               last_update_time_(ros::Time(0.0))
00036         {
00037             for (uint8_t i = 0; i < dof_; i++)
00038             {
00039                 ma_vel_.push_back(new MovingAvgExponential_double_t(integrator_smoothing_));
00040                 ma_pos_.push_back(new MovingAvgExponential_double_t(integrator_smoothing_));
00041             }
00042 
00048         }
00049 
00050         ~SimpsonIntegrator()
00051         {}
00052 
00053         void resetIntegration()
00054         {
00055             // resetting outdated values
00056             vel_last_.clear();
00057             vel_before_last_.clear();
00058 
00059             // resetting moving average
00060             for (unsigned int i = 0; i < dof_; ++i)
00061             {
00062                 ma_vel_[i]->reset();
00063                 ma_pos_[i]->reset();
00064             }
00065         }
00066 
00067         bool updateIntegration(const KDL::JntArray& q_dot_ik,
00068                                const KDL::JntArray& current_q,
00069                                std::vector<double>& pos,
00070                                std::vector<double>& vel)
00071         {
00072             ros::Time now = ros::Time::now();
00073             ros::Duration period = now - last_update_time_;
00074             last_update_time_ = now;
00075 
00076             bool value_valid = false;
00077             pos.clear();
00078             vel.clear();
00079 
00080             // ToDo: Test these conditions and find good thresholds
00081             if (period.toSec() > ros::Duration(0.5).toSec())  // missed about 'max_command_silence'
00082             {
00083                 ROS_WARN_STREAM("reset Integration: " << period.toSec());
00084                 resetIntegration();
00085             }
00086 
00087             // smooth incoming velocities
00088             KDL::JntArray q_dot_avg(dof_);
00091             for (unsigned int i = 0; i < dof_; ++i)
00092             {
00093                 ma_vel_[i]->addElement(q_dot_ik(i));
00094                 double avg_vel = 0.0;
00095                 if (ma_vel_[i]->calcMovingAverage(avg_vel))
00096                 {
00097                     q_dot_avg(i) = avg_vel;
00098                 }
00099                 else
00100                 {
00101                     q_dot_avg(i) = q_dot_ik(i);
00102                 }
00103 
00106             }
00107 
00110             if (!vel_before_last_.empty())
00111             {
00112                 for (unsigned int i = 0; i < dof_; ++i)
00113                 {
00114                     // Simpson
00115                     double integration_value = static_cast<double>(period.toSec() / 6.0 * (vel_before_last_[i] + 4.0 * (vel_before_last_[i] + vel_last_[i]) + vel_before_last_[i] + vel_last_[i] + q_dot_avg(i)) + current_q(i));
00116 
00117                     // smooth outgoing positions
00118                     ma_pos_[i]->addElement(integration_value);
00119                     double avg_pos = 0.0;
00120                     if (!ma_pos_[i]->calcMovingAverage(avg_pos))
00121                     {
00122                         ROS_ERROR("calcMovingAverage failed");
00123                         avg_pos = integration_value;
00124                     }
00125 
00126                     pos.push_back(avg_pos);
00127                     vel.push_back(q_dot_avg(i));
00128 
00131                 }
00132                 value_valid = true;
00133             }
00134 
00135             // Continuously shift the vectors for simpson integration
00136             vel_before_last_.clear();
00137             for (unsigned int i=0; i < vel_last_.size(); ++i)
00138             {
00139                 vel_before_last_.push_back(vel_last_[i]);
00140             }
00141 
00142             vel_last_.clear();
00143             for (unsigned int i=0; i < q_dot_avg.rows(); ++i)
00144             {
00145                 vel_last_.push_back(q_dot_avg(i));
00146             }
00147 
00152 
00153             return value_valid;
00154         }
00155 
00156     private:
00157         std::vector<MovingAvgBase_double_t*> ma_vel_;
00158         std::vector<MovingAvgBase_double_t*> ma_pos_;
00159         uint8_t dof_;
00160         double integrator_smoothing_;
00161         std::vector<double> vel_last_, vel_before_last_;
00162         ros::Time last_update_time_;
00163 
00164         // debug
00170 };
00171 
00172 #endif  // COB_TWIST_CONTROLLER_UTILS_SIMPSON_INTEGRATOR_H


cob_twist_controller
Author(s): Felix Messmer , Marco Bezzon , Christoph Mark , Francisco Moreno
autogenerated on Thu Jun 6 2019 21:19:26