constraint_solver_factory.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 
18 #include <set>
19 #include <ros/ros.h>
20 
30 
33 
40  const Vector6d_t& in_cart_velocities,
41  const JointStates& joint_states,
42  Eigen::MatrixXd& out_jnt_velocities)
43 {
44  out_jnt_velocities = Eigen::MatrixXd::Zero(joint_states.current_q_dot_.rows(),
45  joint_states.current_q_dot_.columns());
46 
47  if (NULL == this->damping_method_)
48  {
49  return -1; // damping method not initialized
50  }
51  else if (NULL == this->solver_factory_)
52  {
53  return -2; // solver factory not initialized
54  }
55  else
56  {
57  // everything seems to be alright!
58  }
59 
60  out_jnt_velocities = this->solver_factory_->calculateJointVelocities(jacobian_data,
61  in_cart_velocities,
62  joint_states,
63  this->damping_method_,
64  this->constraints_);
65 
66  return 0; // success
67 }
68 
73  const LimiterParams& limiter_params,
74  boost::shared_ptr<ISolverFactory>& solver_factory,
75  TaskStackController_t& task_stack_controller)
76 {
77  switch (params.solver)
78  {
79  case DEFAULT_SOLVER:
80  solver_factory.reset(new SolverFactory<UnconstraintSolver>(params, limiter_params, task_stack_controller));
81  break;
82  case WLN:
83  switch (params.constraint_jla)
84  {
85  case JLA_ON:
86  solver_factory.reset(new SolverFactory<WLN_JointLimitAvoidanceSolver>(params, limiter_params, task_stack_controller));
87  break;
88 
89  case JLA_OFF:
90  solver_factory.reset(new SolverFactory<WeightedLeastNormSolver>(params, limiter_params, task_stack_controller));
91  break;
92  }
93  break;
94  case UNIFIED_JLA_SA:
95  solver_factory.reset(new SolverFactory<UnifiedJointLimitSingularitySolver>(params, limiter_params, task_stack_controller));
96  break;
97  case GPM:
98  solver_factory.reset(new SolverFactory<GradientProjectionMethodSolver>(params, limiter_params, task_stack_controller));
99  break;
100  case STACK_OF_TASKS:
101  solver_factory.reset(new SolverFactory<StackOfTasksSolver>(params, limiter_params, task_stack_controller));
102  break;
103  case TASK_2ND_PRIO:
104  solver_factory.reset(new SolverFactory<TaskPrioritySolver>(params, limiter_params, task_stack_controller));
105  break;
106  default:
107  ROS_ERROR("Returning NULL factory due to constraint solver creation error. There is no solver method for %d implemented.",
108  params.solver);
109  return false;
110  }
111 
112  return true;
113 }
114 
115 int8_t ConstraintSolverFactory::resetAll(const TwistControllerParams& params, const LimiterParams& limiter_params)
116 {
117  this->damping_method_.reset(DampingBuilder::createDamping(params));
118  if (NULL == this->damping_method_)
119  {
120  ROS_ERROR("Returning NULL due to damping creation error.");
121  return -1; // error
122  }
123 
124  this->constraints_.clear();
126  limiter_params,
127  this->jnt_to_jac_,
128  this->fk_solver_vel_,
129  this->data_mediator_);
130 
131  for (std::set<ConstraintBase_t>::iterator it = this->constraints_.begin(); it != this->constraints_.end(); ++it)
132  {
133  ROS_DEBUG_STREAM((*it)->getTaskId());
134  }
135 
136  if (!ConstraintSolverFactory::getSolverFactory(params, limiter_params, this->solver_factory_, this->task_stack_controller_))
137  {
138  return -2;
139  }
140 
141  return 0;
142 }
#define NULL
unsigned int rows() const
Eigen::Matrix< double, 6, 1 > Vector6d_t
static bool getSolverFactory(const TwistControllerParams &params, const LimiterParams &limiter_params, boost::shared_ptr< ISolverFactory > &solver_factory, TaskStackController_t &task_stack_controller)
KDL::ChainFkSolverVel_recursive & fk_solver_vel_
int8_t resetAll(const TwistControllerParams &params, const LimiterParams &limiter_params)
boost::shared_ptr< ISolverFactory > solver_factory_
Abstract base class defining interfaces for the creation of a specific solver.
static std::set< ConstraintBase_t > createConstraints(const TwistControllerParams &params, const LimiterParams &limiter_params, KDL::ChainJntToJacSolver &jnt_to_jac_, KDL::ChainFkSolverVel_recursive &fk_solver_vel, CallbackDataMediator &data_mediator)
int8_t calculateJointVelocities(Matrix6Xd_t &jacobian_data, const Vector6d_t &in_cart_velocities, const JointStates &joint_states, Eigen::MatrixXd &out_jnt_velocities)
CallbackDataMediator & data_mediator_
unsigned int columns() const
boost::shared_ptr< DampingBase > damping_method_
std::set< ConstraintBase_t > constraints_
KDL::ChainJntToJacSolver & jnt_to_jac_
#define ROS_DEBUG_STREAM(args)
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6Xd_t
TaskStackController_t & task_stack_controller_
#define ROS_ERROR(...)
static DampingBase * createDamping(const TwistControllerParams &params)
Definition: damping.cpp:25


cob_twist_controller
Author(s): Felix Messmer , Marco Bezzon , Christoph Mark , Francisco Moreno
autogenerated on Thu Apr 8 2021 02:40:00