task_priority_solver.cpp
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 #include <set>
00019 
00020 #include <cob_twist_controller/constraint_solvers/solvers/task_priority_solver.h>
00021 
00026 Eigen::MatrixXd TaskPrioritySolver::solve(const Vector6d_t& in_cart_velocities,
00027                                           const JointStates& joint_states)
00028 {
00029     ros::Time now = ros::Time::now();
00030     double cycle = (now - this->last_time_).toSec();
00031     this->last_time_ = now;
00032 
00033     double derivative_cost_func_value;
00034     double current_cost_func_value;
00035     double activation_gain;
00036     double magnitude;
00037 
00038     Eigen::MatrixXd qdots_out = Eigen::MatrixXd::Zero(this->jacobian_data_.cols(), 1);
00039     Eigen::VectorXd partial_cost_func = Eigen::VectorXd::Zero(this->jacobian_data_.cols());
00040     Eigen::MatrixXd damped_pinv = pinv_calc_.calculate(this->params_, this->damping_, this->jacobian_data_);
00041     Eigen::MatrixXd pinv = pinv_calc_.calculate(this->jacobian_data_);
00042 
00043     Eigen::MatrixXd particular_solution = damped_pinv * in_cart_velocities;
00044 
00045     Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(pinv.rows(), this->jacobian_data_.cols());
00046     Eigen::MatrixXd projector = ident - pinv * this->jacobian_data_;
00047 
00048     KDL::JntArrayVel predict_jnts_vel(joint_states.current_q_.rows());
00049 
00050     // predict next joint states!
00051     for (uint8_t i = 0; i < joint_states.current_q_.rows(); ++i)
00052     {
00053         predict_jnts_vel.q(i) = particular_solution(i, 0) * cycle + joint_states.current_q_(i);
00054         predict_jnts_vel.qdot(i) = particular_solution(i, 0);
00055     }
00056 
00057     if (this->constraints_.size() > 0)
00058     {
00059         for (std::set<ConstraintBase_t>::iterator it = this->constraints_.begin(); it != this->constraints_.end(); ++it)
00060         {
00061             (*it)->update(joint_states, predict_jnts_vel, this->jacobian_data_);
00062             current_cost_func_value = (*it)->getValue();
00063             derivative_cost_func_value = (*it)->getDerivativeValue();
00064             partial_cost_func = (*it)->getPartialValues();  // Equal to (partial g) / (partial q) = J_g
00065             activation_gain = (*it)->getActivationGain();
00066             magnitude = (*it)->getSelfMotionMagnitude(Eigen::MatrixXd::Zero(1, 1), Eigen::MatrixXd::Zero(1, 1));  // not necessary to pass valid values here.
00067 
00068             ROS_INFO_STREAM("activation_gain: " << activation_gain);
00069             ROS_INFO_STREAM("smm: " << magnitude);
00070         }
00071 
00072         Eigen::MatrixXd jac_inv_2nd_term = Eigen::MatrixXd::Zero(projector.cols(), partial_cost_func.cols());
00073         if (activation_gain > 0.0)
00074         {
00075             Eigen::MatrixXd tmp_matrix = partial_cost_func.transpose() * projector;
00076             jac_inv_2nd_term = pinv_calc_.calculate(tmp_matrix);
00077         }
00078 
00079         Eigen::MatrixXd m_derivative_cost_func_value = derivative_cost_func_value * Eigen::MatrixXd::Identity(1, 1);
00080         qdots_out = particular_solution + this->params_.k_H * activation_gain * jac_inv_2nd_term * (magnitude * m_derivative_cost_func_value - partial_cost_func.transpose() * particular_solution);
00081     }
00082     else
00083     {
00084         qdots_out = particular_solution;
00085         ROS_ERROR_STREAM("Should not occur solution: " << std::endl << qdots_out);
00086     }
00087 
00088     // Eigen::MatrixXd qdots_out = particular_solution + homogeneousSolution; // weighting with k_H is done in loop
00089     return qdots_out;
00090 }
00091 


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