constraint_solver_factory.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 #include <ros/ros.h>
00020 
00021 #include <cob_twist_controller/constraint_solvers/constraint_solver_factory.h>
00022 #include "cob_twist_controller/constraint_solvers/solvers/constraint_solver_base.h"
00023 #include "cob_twist_controller/constraint_solvers/solvers/unconstraint_solver.h"
00024 #include "cob_twist_controller/constraint_solvers/solvers/wln_joint_limit_avoidance_solver.h"
00025 #include "cob_twist_controller/constraint_solvers/solvers/weighted_least_norm_solver.h"
00026 #include "cob_twist_controller/constraint_solvers/solvers/gradient_projection_method_solver.h"
00027 #include "cob_twist_controller/constraint_solvers/solvers/task_priority_solver.h"
00028 #include "cob_twist_controller/constraint_solvers/solvers/stack_of_tasks_solver.h"
00029 #include "cob_twist_controller/constraint_solvers/solvers/unified_joint_limit_singularity_solver.h"
00030 
00031 #include "cob_twist_controller/damping_methods/damping.h"
00032 #include "cob_twist_controller/constraints/constraint.h"
00033 
00039 int8_t ConstraintSolverFactory::calculateJointVelocities(Matrix6Xd_t& jacobian_data,
00040                                                          const Vector6d_t& in_cart_velocities,
00041                                                          const JointStates& joint_states,
00042                                                          Eigen::MatrixXd& out_jnt_velocities)
00043 {
00044     out_jnt_velocities = Eigen::MatrixXd::Zero(joint_states.current_q_dot_.rows(),
00045                                                joint_states.current_q_dot_.columns());
00046 
00047     if (NULL == this->damping_method_)
00048     {
00049         return -1;  // damping method not initialized
00050     }
00051     else if (NULL == this->solver_factory_)
00052     {
00053         return -2;  // solver factory not initialized
00054     }
00055     else
00056     {
00057         // everything seems to be alright!
00058     }
00059 
00060     out_jnt_velocities = this->solver_factory_->calculateJointVelocities(jacobian_data,
00061                                                                          in_cart_velocities,
00062                                                                          joint_states,
00063                                                                          this->damping_method_,
00064                                                                          this->constraints_);
00065 
00066     return 0;   // success
00067 }
00068 
00072 bool ConstraintSolverFactory::getSolverFactory(const TwistControllerParams& params,
00073                                                const LimiterParams& limiter_params,
00074                                                boost::shared_ptr<ISolverFactory>& solver_factory,
00075                                                TaskStackController_t& task_stack_controller)
00076 {
00077     switch (params.solver)
00078     {
00079         case DEFAULT_SOLVER:
00080             solver_factory.reset(new SolverFactory<UnconstraintSolver>(params, limiter_params, task_stack_controller));
00081             break;
00082         case WLN:
00083             switch (params.constraint_jla)
00084             {
00085                 case JLA_ON:
00086                     solver_factory.reset(new SolverFactory<WLN_JointLimitAvoidanceSolver>(params, limiter_params, task_stack_controller));
00087                 break;
00088 
00089                 case JLA_OFF:
00090                     solver_factory.reset(new SolverFactory<WeightedLeastNormSolver>(params, limiter_params, task_stack_controller));
00091                 break;
00092             }
00093             break;
00094         case UNIFIED_JLA_SA:
00095             solver_factory.reset(new SolverFactory<UnifiedJointLimitSingularitySolver>(params, limiter_params, task_stack_controller));
00096             break;
00097         case GPM:
00098             solver_factory.reset(new SolverFactory<GradientProjectionMethodSolver>(params, limiter_params, task_stack_controller));
00099             break;
00100         case STACK_OF_TASKS:
00101             solver_factory.reset(new SolverFactory<StackOfTasksSolver>(params, limiter_params, task_stack_controller));
00102             break;
00103         case TASK_2ND_PRIO:
00104             solver_factory.reset(new SolverFactory<TaskPrioritySolver>(params, limiter_params, task_stack_controller));
00105             break;
00106         default:
00107             ROS_ERROR("Returning NULL factory due to constraint solver creation error. There is no solver method for %d implemented.",
00108                       params.solver);
00109             return false;
00110     }
00111 
00112     return true;
00113 }
00114 
00115 int8_t ConstraintSolverFactory::resetAll(const TwistControllerParams& params, const LimiterParams& limiter_params)
00116 {
00117     this->damping_method_.reset(DampingBuilder::createDamping(params));
00118     if (NULL == this->damping_method_)
00119     {
00120         ROS_ERROR("Returning NULL due to damping creation error.");
00121         return -1;  // error
00122     }
00123 
00124     this->constraints_.clear();
00125     this->constraints_ = ConstraintsBuilder_t::createConstraints(params,
00126                                                                  limiter_params,
00127                                                                  this->jnt_to_jac_,
00128                                                                  this->fk_solver_vel_,
00129                                                                  this->data_mediator_);
00130 
00131     for (std::set<ConstraintBase_t>::iterator it = this->constraints_.begin(); it != this->constraints_.end(); ++it)
00132     {
00133         ROS_DEBUG_STREAM((*it)->getTaskId());
00134     }
00135 
00136     if (!ConstraintSolverFactory::getSolverFactory(params, limiter_params, this->solver_factory_, this->task_stack_controller_))
00137     {
00138         return -2;
00139     }
00140 
00141     return 0;
00142 }


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