sr_motor_hand_lib.cpp
Go to the documentation of this file.
1 
29 #include <algorithm>
30 #include <utility>
31 #include <string>
32 #include <vector>
33 #include <boost/foreach.hpp>
34 #include <boost/algorithm/string.hpp>
35 
36 #include <sr_utilities/sr_math_utils.hpp>
37 
39 
40 using std::vector;
41 using std::string;
42 using std::pair;
43 using std::ostringstream;
44 using sr_actuator::SrMotorActuator;
50 using boost::shared_ptr;
51 using boost::static_pointer_cast;
52 
53 namespace shadow_robot
54 {
55  template<class StatusType, class CommandType>
57 
58  template<class StatusType, class CommandType>
59  const char *SrMotorHandLib<StatusType,
60  CommandType>::human_readable_motor_data_types[nb_motor_data] =
61  {
62  "sgl", "sgr",
63  "pwm", "flags",
64  "current",
65  "voltage",
66  "temperature",
67  "can_num_received",
68  "can_num_transmitted",
69  "slow_data",
70  "can_error_counters",
71  "pterm",
72  "iterm",
73  "dterm"
74  };
75 
76  template <class StatusType, class CommandType>
78  {
93  };
94 
95  template<class StatusType, class CommandType>
96  SrMotorHandLib<StatusType, CommandType>::SrMotorHandLib(hardware_interface::HardwareInterface *hw, ros::NodeHandle nh,
97  ros::NodeHandle nhtilde, string device_id,
98  string joint_prefix) :
99  SrMotorRobotLib<StatusType, CommandType>(hw, nh, nhtilde, device_id, joint_prefix)
100  {
101  // read the motor polling frequency from the parameter server
102  this->motor_update_rate_configs_vector = this->read_update_rate_configs("motor_data_update_rate/", nb_motor_data,
103  human_readable_motor_data_types,
104  motor_data_types);
105  this->motor_updater_ = shared_ptr<MotorUpdater<CommandType> >(
106  new MotorUpdater<CommandType>(this->motor_update_rate_configs_vector,
108 
109  // @todo read this from config/EEProm?
110  vector<JointToSensor> joint_to_sensor_vect = this->read_joint_to_sensor_mapping();
111 
112  // initializing the joints vector
113  vector<string> joint_names_tmp;
114  vector<int> motor_ids = read_joint_to_motor_mapping();
115 
116  ROS_ASSERT(motor_ids.size() == JOINTS_NUM_0220);
117  ROS_ASSERT(joint_to_sensor_vect.size() == JOINTS_NUM_0220);
118 
119  for (unsigned int i = 0; i < JOINTS_NUM_0220; ++i)
120  {
121  joint_names_tmp.push_back(string(joint_names[i]));
122  }
123  initialize(joint_names_tmp, motor_ids, joint_to_sensor_vect);
124  // Initialize the motor data checker
125  this->motor_data_checker = shared_ptr<MotorDataChecker>(
126  new MotorDataChecker(this->joints_vector, this->motor_updater_->initialization_configs_vector));
127  }
128 
129  template<class StatusType, class CommandType>
131  vector<int> actuator_ids,
132  vector<JointToSensor> joint_to_sensors)
133  {
134  for (unsigned int index = 0; index < joint_names.size(); ++index)
135  {
136  // add the joint and the vector of joints.
137  Joint joint;
138  float tau;
139 
140  // read the parameters tau from the parameter server
141  ostringstream full_param;
142  string act_name = boost::to_lower_copy(joint_names[index]);
143  full_param << act_name << "/pos_filter/tau";
144  this->nodehandle_.template param<float>(full_param.str(), tau, 0.05);
145  full_param.str("");
146  joint.pos_filter = sr_math_utils::filters::LowPassFilter(tau);
147 
148  // update the joint variables
149  joint.joint_name = joint_names[index];
150  joint.joint_to_sensor = joint_to_sensors[index];
151 
152  if (actuator_ids[index] != -1)
153  {
154  joint.has_actuator = true;
155  shared_ptr<MotorWrapper> motor_wrapper(new MotorWrapper());
156  joint.actuator_wrapper = motor_wrapper;
157  motor_wrapper->motor_id = actuator_ids[index];
158  motor_wrapper->actuator = static_cast<SrMotorActuator *> (this->hw_->getActuator(
159  this->joint_prefix_ + joint.joint_name));
160  if (motor_wrapper->actuator == NULL)
161  {
162  ROS_WARN_STREAM("No actuator found for joint " <<
163  this->joint_prefix_ + joint.joint_name <<
164  " (check the robot_description contains it)");
165  joint.has_actuator = false;
166  continue;
167  }
168 
169  ostringstream ss;
170  ss << "change_force_PID_" << joint_names[index];
171  // initialize the force pid service
172  // NOTE: the template keyword is needed to avoid a compiler complaint apparently due to the fact that
173  // we are using an explicit template function inside this template class
174  motor_wrapper->force_pid_service =
175  this->nh_tilde.template advertiseService<sr_robot_msgs::ForceController::Request,
176  sr_robot_msgs::ForceController::Response>(ss.str().c_str(),
177  boost::bind(
178  &SrMotorHandLib<StatusType,
179  CommandType>::force_pid_callback,
180  this, _1, _2,
181  motor_wrapper->motor_id));
182 
183  ss.str("");
184  ss << "reset_motor_" << joint_names[index];
185  // initialize the reset motor service
186  motor_wrapper->reset_motor_service =
187  this->nh_tilde.template advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>(
188  ss.str().c_str(),
190  pair<int, string>(motor_wrapper->motor_id, joint.joint_name)));
191  }
192  else
193  { // no motor associated to this joint
194  joint.has_actuator = false;
195  }
196 
197  this->joints_vector.push_back(joint);
198  } // end for joints.
199  }
200 
201  template<class StatusType, class CommandType>
203  std_srvs::Empty::Response &response,
204  pair<int, string> joint)
205  {
206  ROS_INFO_STREAM(" resetting " << joint.second << " (" << joint.first << ")");
207 
208  this->reset_motors_queue.push(joint.first);
209 
210  // wait a few secs for the reset to be sent then resend the pids
211  string joint_name = joint.second;
212 
213  pid_timers[joint_name] = this->nh_tilde.createTimer(ros::Duration(3.0),
214  boost::bind(&SrMotorHandLib::resend_pids, this, joint_name,
215  joint.first),
216  true);
217 
218  return true;
219  }
220 
221  template<class StatusType, class CommandType>
222  void SrMotorHandLib<StatusType, CommandType>::resend_pids(string joint_name, int motor_index)
223  {
224  // read the parameters from the parameter server and set the pid
225  // values.
226  ostringstream full_param;
227 
228  int f, p, i, d, imax, max_pwm, sg_left, sg_right, deadband, sign, torque_limit, torque_limiter_gain;
229  string act_name = boost::to_lower_copy(joint_name);
230 
231  full_param << act_name << "/pid/f";
232  this->nodehandle_.template param<int>(full_param.str(), f, 0);
233  full_param.str("");
234  full_param << act_name << "/pid/p";
235  this->nodehandle_.template param<int>(full_param.str(), p, 0);
236  full_param.str("");
237  full_param << act_name << "/pid/i";
238  this->nodehandle_.template param<int>(full_param.str(), i, 0);
239  full_param.str("");
240  full_param << act_name << "/pid/d";
241  this->nodehandle_.template param<int>(full_param.str(), d, 0);
242  full_param.str("");
243  full_param << act_name << "/pid/imax";
244  this->nodehandle_.template param<int>(full_param.str(), imax, 0);
245  full_param.str("");
246  full_param << act_name << "/pid/max_pwm";
247  this->nodehandle_.template param<int>(full_param.str(), max_pwm, 0);
248  full_param.str("");
249  full_param << act_name << "/pid/sgleftref";
250  this->nodehandle_.template param<int>(full_param.str(), sg_left, 0);
251  full_param.str("");
252  full_param << act_name << "/pid/sgrightref";
253  this->nodehandle_.template param<int>(full_param.str(), sg_right, 0);
254  full_param.str("");
255  full_param << act_name << "/pid/deadband";
256  this->nodehandle_.template param<int>(full_param.str(), deadband, 0);
257  full_param.str("");
258  full_param << act_name << "/pid/sign";
259  this->nodehandle_.template param<int>(full_param.str(), sign, 0);
260  full_param.str("");
261  full_param << act_name << "/pid/torque_limit";
262  this->nodehandle_.template param<int>(full_param.str(), torque_limit, 0);
263  full_param.str("");
264  full_param << act_name << "/pid/torque_limiter_gain";
265  this->nodehandle_.template param<int>(full_param.str(), torque_limiter_gain, 0);
266  full_param.str("");
267 
268 
269  sr_robot_msgs::ForceController::Request pid_request;
270  pid_request.maxpwm = max_pwm;
271  pid_request.sgleftref = sg_left;
272  pid_request.sgrightref = sg_right;
273  pid_request.f = f;
274  pid_request.p = p;
275  pid_request.i = i;
276  pid_request.d = d;
277  pid_request.imax = imax;
278  pid_request.deadband = deadband;
279  pid_request.sign = sign;
280  pid_request.torque_limit = torque_limit;
281  pid_request.torque_limiter_gain = torque_limiter_gain;
282  sr_robot_msgs::ForceController::Response pid_response;
283  bool pid_success = force_pid_callback(pid_request, pid_response, motor_index);
284 
285  // setting the backlash compensation (on or off)
286  bool backlash_compensation;
287  full_param << act_name << "/backlash_compensation";
288  this->nodehandle_.template param<bool>(full_param.str(), backlash_compensation, true);
289  full_param.str("");
290  sr_robot_msgs::ChangeMotorSystemControls::Request backlash_request;
291  sr_robot_msgs::MotorSystemControls motor_sys_ctrl;
292  motor_sys_ctrl.motor_id = motor_index;
293  motor_sys_ctrl.enable_backlash_compensation = backlash_compensation;
294 
295  if (!backlash_compensation)
296  ROS_INFO_STREAM("Setting backlash compensation to OFF for joint " << act_name);
297 
298  backlash_request.motor_system_controls.push_back(motor_sys_ctrl);
299  sr_robot_msgs::ChangeMotorSystemControls::Response backlash_response;
300  bool backlash_success = this->motor_system_controls_callback_(backlash_request, backlash_response);
301 
302  if (!pid_success)
303  ROS_WARN_STREAM("Didn't load the force pid settings for the motor in joint " << act_name);
304  if (!backlash_success)
305  ROS_WARN_STREAM("Didn't set the backlash compensation correctly for the motor in joint " << act_name);
306  }
307 
308  template<class StatusType, class CommandType>
309  bool SrMotorHandLib<StatusType, CommandType>::force_pid_callback(sr_robot_msgs::ForceController::Request &request,
310  sr_robot_msgs::ForceController::Response &response,
311  int motor_index)
312  {
313  ROS_INFO_STREAM("Received new force PID parameters for motor " << motor_index);
314 
315  // Check the parameters are in the correct ranges
316  if (motor_index > 20)
317  {
318  ROS_WARN_STREAM(" Wrong motor index specified: " << motor_index);
319  response.configured = false;
320  return false;
321  }
322 
323  if (!((request.maxpwm >= MOTOR_DEMAND_PWM_RANGE_MIN) &&
324  (request.maxpwm <= MOTOR_DEMAND_PWM_RANGE_MAX))
325  )
326  {
327  ROS_WARN_STREAM(" pid parameter maxpwm is out of range : " << request.maxpwm << " -> not in [" <<
329  response.configured = false;
330  return false;
331  }
332 
333  if (!((request.f >= MOTOR_CONFIG_F_RANGE_MIN) &&
334  (request.maxpwm <= MOTOR_CONFIG_F_RANGE_MAX))
335  )
336  {
337  ROS_WARN_STREAM(" pid parameter f is out of range : " << request.f << " -> not in [" <<
339  response.configured = false;
340  return false;
341  }
342 
343  if (!((request.p >= MOTOR_CONFIG_P_RANGE_MIN) &&
344  (request.p <= MOTOR_CONFIG_P_RANGE_MAX))
345  )
346  {
347  ROS_WARN_STREAM(" pid parameter p is out of range : " << request.p << " -> not in [" <<
349  response.configured = false;
350  return false;
351  }
352 
353  if (!((request.i >= MOTOR_CONFIG_I_RANGE_MIN) &&
354  (request.i <= MOTOR_CONFIG_I_RANGE_MAX))
355  )
356  {
357  ROS_WARN_STREAM(" pid parameter i is out of range : " << request.i << " -> not in [" <<
359  response.configured = false;
360  return false;
361  }
362 
363  if (!((request.d >= MOTOR_CONFIG_D_RANGE_MIN) &&
364  (request.d <= MOTOR_CONFIG_D_RANGE_MAX))
365  )
366  {
367  ROS_WARN_STREAM(" pid parameter d is out of range : " << request.d << " -> not in [" <<
369  response.configured = false;
370  return false;
371  }
372 
373  if (!((request.imax >= MOTOR_CONFIG_IMAX_RANGE_MIN) &&
374  (request.imax <= MOTOR_CONFIG_IMAX_RANGE_MAX))
375  )
376  {
377  ROS_WARN_STREAM(" pid parameter imax is out of range : " << request.imax << " -> not in [" <<
379  response.configured = false;
380  return false;
381  }
382 
383  if (!((request.deadband >= MOTOR_CONFIG_DEADBAND_RANGE_MIN) &&
384  (request.deadband <= MOTOR_CONFIG_DEADBAND_RANGE_MAX))
385  )
386  {
387  ROS_WARN_STREAM(" pid parameter deadband is out of range : " << request.deadband << " -> not in [" <<
389  response.configured = false;
390  return false;
391  }
392 
393  if (!((request.sign >= MOTOR_CONFIG_SIGN_RANGE_MIN) &&
394  (request.sign <= MOTOR_CONFIG_SIGN_RANGE_MAX))
395  )
396  {
397  ROS_WARN_STREAM(" pid parameter sign is out of range : " << request.sign << " -> not in [" <<
399  response.configured = false;
400  return false;
401  }
402 
403  if (!((request.torque_limiter_gain >= MOTOR_DEMAND_TORQUE_LIMITER_GAIN_MIN) &&
404  (request.torque_limiter_gain <= MOTOR_DEMAND_TORQUE_LIMITER_GAIN_MAX)))
405  {
406  ROS_WARN_STREAM(" torque limiter gain out or range : " << request.torque_limiter_gain << " -> not in [" <<
409  response.configured = false;
410  return false;
411  }
412 
413 
414  // ok, the parameters sent are coherent, send the demand to the motor.
415  this->generate_force_control_config(motor_index, request.maxpwm, request.sgleftref,
416  request.sgrightref, request.f, request.p, request.i,
417  request.d, request.imax, request.deadband, request.sign,
418  request.torque_limit, request.torque_limiter_gain);
419 
420  update_force_control_in_param_server(find_joint_name(motor_index), request.maxpwm, request.sgleftref,
421  request.sgrightref, request.f, request.p, request.i,
422  request.d, request.imax, request.deadband, request.sign,
423  request.torque_limit, request.torque_limiter_gain);
424  response.configured = true;
425 
426  // Reinitialize motors information
427  this->reinitialize_motors();
428 
429  return true;
430  }
431 
432  template<class StatusType, class CommandType>
434  {
435  for (vector<Joint>::iterator joint = this->joints_vector.begin();
436  joint != this->joints_vector.end();
437  ++joint)
438  {
439  if (joint->has_actuator && static_pointer_cast<MotorWrapper>(joint->actuator_wrapper)->motor_id == motor_index)
440  {
441  return joint->joint_name;
442  }
443  }
444  ROS_ERROR("Could not find joint name for motor index: %d", motor_index);
445  return "";
446  }
447 
448  template<class StatusType, class CommandType>
450  string joint_name, int max_pwm, int sg_left, int sg_right, int f, int p, int i,
451  int d, int imax, int deadband, int sign, int torque_limit, int torque_limiter_gain)
452  {
453  if (joint_name != "")
454  {
455  ostringstream full_param;
456  string act_name = boost::to_lower_copy(joint_name);
457 
458  full_param << act_name << "/pid/f";
459  this->nodehandle_.setParam(full_param.str(), f);
460  full_param.str("");
461  full_param << act_name << "/pid/p";
462  this->nodehandle_.setParam(full_param.str(), p);
463  full_param.str("");
464  full_param << act_name << "/pid/i";
465  this->nodehandle_.setParam(full_param.str(), i);
466  full_param.str("");
467  full_param << act_name << "/pid/d";
468  this->nodehandle_.setParam(full_param.str(), d);
469  full_param.str("");
470  full_param << act_name << "/pid/imax";
471  this->nodehandle_.setParam(full_param.str(), imax);
472  full_param.str("");
473  full_param << act_name << "/pid/max_pwm";
474  this->nodehandle_.setParam(full_param.str(), max_pwm);
475  full_param.str("");
476  full_param << act_name << "/pid/sgleftref";
477  this->nodehandle_.setParam(full_param.str(), sg_left);
478  full_param.str("");
479  full_param << act_name << "/pid/sgrightref";
480  this->nodehandle_.setParam(full_param.str(), sg_right);
481  full_param.str("");
482  full_param << act_name << "/pid/deadband";
483  this->nodehandle_.setParam(full_param.str(), deadband);
484  full_param.str("");
485  full_param << act_name << "/pid/sign";
486  this->nodehandle_.setParam(full_param.str(), sign);
487  full_param.str("");
488  full_param << act_name << "/pid/torque_limit";
489  this->nodehandle_.setParam(full_param.str(), torque_limit);
490  full_param.str("");
491  full_param << act_name << "/pid/torque_limiter_gain";
492  this->nodehandle_.setParam(full_param.str(), torque_limiter_gain);
493  }
494  }
495 
496  template<class StatusType, class CommandType>
498  {
499  vector<int> motor_ids;
500  string param_name = "joint_to_motor_mapping";
501 
502  XmlRpc::XmlRpcValue mapping;
503  this->nodehandle_.getParam(param_name, mapping);
505  // iterate on all the joints
506  for (int32_t i = 0; i < mapping.size(); ++i)
507  {
508  ROS_ASSERT(mapping[i].getType() == XmlRpc::XmlRpcValue::TypeInt);
509  motor_ids.push_back(static_cast<int> (mapping[i]));
510  }
511 
512  return motor_ids;
513  } // end read_joint_to_motor_mapping
514 
515 
516 #ifdef DEBUG_PUBLISHER
517  template <class StatusType, class CommandType>
518  bool SrMotorHandLib<StatusType,
519  CommandType>::set_debug_data_to_publish(sr_robot_msgs::SetDebugData::Request& request,
520  sr_robot_msgs::SetDebugData::Response& response)
521  {
522  // check if the publisher_index is correct
523  if (request.publisher_index < this->nb_debug_publishers_const)
524  {
525  if (request.motor_index > NUM_MOTORS)
526  {
527  response.success = false;
528  return false;
529  }
530  if (request.motor_data_type > 0)
531  {
532  if ((request.motor_data_type < MOTOR_DATA_SGL) ||
533  (request.motor_data_type > MOTOR_DATA_DTERM))
534  {
535  response.success = false;
536  return false;
537  }
538  }
539  if (!this->debug_mutex.timed_lock(boost::posix_time::microseconds(this->debug_mutex_lock_wait_time)))
540  {
541  response.success = false;
542  return false;
543  }
544 
545  this->debug_motor_indexes_and_data[request.publisher_index] = shared_ptr<pair<int, int> >(new pair<int, int>());
546 
547  this->debug_motor_indexes_and_data[request.publisher_index]->first = request.motor_index;
548  this->debug_motor_indexes_and_data[request.publisher_index]->second = request.motor_data_type;
549  this->debug_mutex.unlock();
550  }
551  else
552  {
553  response.success = false;
554  return false;
555  }
556 
557  response.success = true;
558  return true;
559  }
560 #endif
561 
562  // Only to ensure that the template class is compiled for the types we are interested in
563  template
565 
566  template
568 
569  template
571 
572 
573 } // namespace shadow_robot
574 
575 /* For the emacs weenies in the crowd.
576 Local Variables:
577  c-basic-offset: 2
578 End:
579 */
d
JointToSensor joint_to_sensor
MOTOR_DATA_SGR
#define MOTOR_CONFIG_D_RANGE_MAX
MOTOR_DATA_PTERM
#define NUM_MOTORS
f
ROSCONSOLE_DECL void initialize()
#define MOTOR_CONFIG_IMAX_RANGE_MAX
ServiceServer advertiseService(ros::NodeHandle n, std::string name, boost::function< bool(const typename ActionSpec::_action_goal_type::_goal_type &, typename ActionSpec::_action_result_type::_result_type &result)> service_cb)
#define MOTOR_CONFIG_I_RANGE_MIN
#define MOTOR_CONFIG_D_RANGE_MIN
MOTOR_DATA_SGL
#define MOTOR_CONFIG_SIGN_RANGE_MAX
#define MOTOR_CONFIG_P_RANGE_MIN
#define MOTOR_DEMAND_TORQUE_LIMITER_GAIN_MIN
MOTOR_DATA_CURRENT
MOTOR_DATA_ITERM
MOTOR_DATA_CAN_NUM_RECEIVED
unsigned int int32u
#define MOTOR_CONFIG_P_RANGE_MAX
#define MOTOR_DEMAND_PWM_RANGE_MIN
#define MOTOR_DEMAND_TORQUE_LIMITER_GAIN_MAX
#define MOTOR_DEMAND_PWM_RANGE_MAX
#define MOTOR_CONFIG_F_RANGE_MAX
Type const & getType() const
#define MOTOR_CONFIG_F_RANGE_MIN
#define MOTOR_CONFIG_DEADBAND_RANGE_MIN
#define MOTOR_CONFIG_DEADBAND_RANGE_MAX
MOTOR_DATA_VOLTAGE
sr_math_utils::filters::LowPassFilter pos_filter
MOTOR_DATA_CAN_ERROR_COUNTERS
#define MOTOR_CONFIG_SIGN_RANGE_MIN
MOTOR_DATA_CAN_NUM_TRANSMITTED
MOTOR_DATA_FLAGS
#define ROS_WARN_STREAM(args)
boost::shared_ptr< SrActuatorWrapper > actuator_wrapper
#define ROS_INFO_STREAM(args)
static const int32u motor_data_types[]
#define MOTOR_CONFIG_I_RANGE_MAX
SrMotorHandLib(hardware_interface::HardwareInterface *hw, ros::NodeHandle nh, ros::NodeHandle nhtilde, std::string device_id, std::string joint_prefix)
MOTOR_DATA_PWM
#define JOINTS_NUM_0220
MOTOR_DATA_SLOW_MISC
#define ROS_ASSERT(cond)
#define MOTOR_CONFIG_IMAX_RANGE_MIN
#define ROS_ERROR(...)
MOTOR_DATA_DTERM
const std::string response
MOTOR_DATA_TEMPERATURE


sr_robot_lib
Author(s): Ugo Cupcic, Toni Oliver
autogenerated on Mon Feb 28 2022 23:50:43