mia_hand_gazebo  rel 1.0.0
Mia_hw_sim.cpp
Go to the documentation of this file.
1 /*********************************************************************************************************
2  Author: Prensilia srl
3  Desc: Hardware Interface for mia simulated robot in Gazebo
4 
5  HEAD
6  version 1.7.7
7 
8 **********************************************************************************************************/
9 
10 
11 
13 
14 
15 namespace
16 {
20  double clamp(const double val, const double min_val, const double max_val)
21  {
22  return std::min(std::max(val, min_val), max_val);
23  }
24 
25 
26 
27 }
28 
29 namespace mia
30 {
31  bool MiaHWSim::initSim(const std::string& robot_namespace,
32  ros::NodeHandle model_nh,
33  gazebo::physics::ModelPtr parent_model,
34  const urdf::Model *const urdf_model,
35  std::vector<transmission_interface::TransmissionInfo> transmissions)
36  {
37 
38  ROS_INFO_NAMED("mia_hw_sim","Initialization of Mia Hw_Sim" );
39 
40  // Initialize passive joint thumb_opp + Index_fle
41  const bool load = false;
42  MyTh_opp_passiveJoint.init(load); // initialize without loading now URDF
43 
44  // getJointLimits() searches joint_limit_nh for joint limit parameters. The format of each
45  // parameter's name is "joint_limits/<joint name>". An example is "joint_limits/axle_joint".
46  const ros::NodeHandle joint_limit_nh(model_nh);
47 
48  // Resize vectors to our mia hand DOF
49  write_counter = 0;
50  n_dof_ = transmissions.size();
51  joint_names_.resize(n_dof_);
52  joint_types_.resize(n_dof_);
57  pid_controllers_.resize(n_dof_);
58  joint_position_.resize(n_dof_);
59  joint_velocity_.resize(n_dof_);
60  joint_effort_.resize(n_dof_);
64 
69 
71 
72  //*
73  // Check if the loaded URDF has middle little ring independt or not.
74  mrl_united = true;
75  for(unsigned int j=0; j < n_dof_; j++)
76  {
77  // Check if in the Gazebo URDF there are j_middle_flex or j_ring_flex or j_little_flex
78  // If any of them are found --> URDF mode mrl_united true
79  const std::string jname = transmissions[j].joints_[0].name_;
80  if (jname == "j_middle_fle" )
81  {
83  mrl_united = false;
84  break;
85  }
86  else if (jname == "j_ring_fle" )
87  {
88  joints_ii.j_mrl_2 = j;
89  mrl_united = false;
90  break;
91  }
92  else if (jname == "j_little_fle" )
93  {
94  joints_ii.j_mrl_3 = j;
95  mrl_united = false;
96  break;
97  }
98  }
99 
100  // Print the result
101  if(!mrl_united)
102  {
103  ROS_INFO_STREAM_NAMED("mia_hw_sim"," middle ring and little as indipendent joints ");
104  }
105  else
106  {
107  ROS_INFO_STREAM_NAMED("mia_hw_sim"," middle ring and little as physically united joints ") ;
108  }
109 
111  // Initialize values
112  for(unsigned int j=0; j < n_dof_; j++)
113  {
114 
115  // Check that this transmission has one joint
116  if(transmissions[j].joints_.size() == 0)
117  {
118  ROS_WARN_STREAM_NAMED("mia_hw_sim","Transmission " << transmissions[j].name_
119  << " has no associated joints.");
120  continue;
121  }
122  else if(transmissions[j].joints_.size() > 1)
123  {
124  ROS_WARN_STREAM_NAMED("mia_hw_sim","Transmission " << transmissions[j].name_
125  << " has more than one joint. Currently the default robot hardware simulation "
126  << " interface only supports one.");
127  continue;
128  }
129 
130  /* Get info about the used hardware interface from the tag hardware interface.
131  Or if the hardware interface is empty try to finf this info within the actuator tag
132  (deprectaed option) */
133  std::vector<std::string> joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_;
134 
135  if (joint_interfaces.empty() &&
136  !(transmissions[j].actuators_.empty()) &&
137  !(transmissions[j].actuators_[0].hardware_interfaces_.empty())) // TODO: Deprecate HW interface specification in actuators in ROS J
138  {
139  joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_;
140  ROS_WARN_STREAM_NAMED("mia_hw_sim", "The <hardware_interface> element of tranmission " <<
141  transmissions[j].name_ << " should be nested inside the <joint> element, not <actuator>. " <<
142  "The transmission will be properly loaded, but please update " <<
143  "your robot model to remain compatible with future versions of the plugin.");
144  }
145  if (joint_interfaces.empty())
146  {
147  ROS_WARN_STREAM_NAMED("mia_hw_sim", "Joint " << transmissions[j].joints_[0].name_ <<
148  " of transmission " << transmissions[j].name_ << " does not specify any hardware interface. " <<
149  "Not adding it to the robot hardware simulation.");
150  continue;
151  }
152  else if (joint_interfaces.size() > 1)
153  {
154  ROS_WARN_STREAM_NAMED("mia_hw_sim", "Joint " << transmissions[j].joints_[0].name_ <<
155  " of transmission " << transmissions[j].name_ << " specifies multiple hardware interfaces. " <<
156  "Currently the default robot hardware simulation interface only supports one. Using the first entry!");
157  //continue;
158  }
159 
160 
161  /* Add data from transmission and save the index number of each joint of
162  the simulated Mia hand */
163 
164  const std::string temp_joint_name = transmissions[j].joints_[0].name_;
165 
166  if (temp_joint_name == "j_thumb_opp" )
167  {
169  }
170  else if (temp_joint_name == "j_thumb_fle" )
171  {
173  }
174  else if (temp_joint_name == "j_index_fle" )
175  {
177  }
178  else if (temp_joint_name == "j_mrl_fle" )
179  {
180  joints_ii.j_mrl_flex = j;
181  }
182  else if (temp_joint_name == "j_ring_fle" )
183  {
184  joints_ii.j_mrl_2 = j;
185  }
186  else if (temp_joint_name == "j_little_fle" )
187  {
188  joints_ii.j_mrl_3 = j;
189  }
190  else
191  {
192  //ROS_INFO_STREAM_NAMED("mia_hw_sim","Additional joint that are not part of the MiaHand has been loaded: "
193  // << temp_joint_name);
194  }
195 
196  // Add data from transmission
197  joint_names_[j] = transmissions[j].joints_[0].name_;
198  joint_position_[j] = 0;
199  joint_velocity_[j] = 0.0;
200  joint_effort_[j] = 1.0; // N/m for continuous joints
201  joint_effort_command_[j] = 0.0;
202  joint_position_command_[j] = 0.0;
203  joint_velocity_command_[j] = 0.0;
204 
205  const std::string& hardware_interface = joint_interfaces.front(); // returns the reference to the first element in the vector
206 
207  // Debug
208  //ROS_INFO_STREAM_NAMED("mia_hw_sim", "loading Joint " << joint_names_[j]<< " with the index " << j );
209 
210 
211  //*
212  // Register Joint states interface for all the joints of the simulated hardware
213  js_interface_.registerHandle(hardware_interface::JointStateHandle(
215  &joint_velocity_[j], &joint_effort_[j]));
216 
217  // Select and register the correct command interface for all the joints of the simulated hardware.
218  std::vector< hardware_interface::JointHandle> joint_handle;
219 
220  if(hardware_interface == "EffortJointInterface" || hardware_interface =="hardware_interface/EffortJointInterface" )
221  {
222  ROS_INFO_STREAM_NAMED("mia_hw_sim", " Joint " << joint_names_[j]<< " EFFORT" );
223 
224  joint_handle.resize(1);
225  List_joint_control_methods_[j].resize(1);
226 
227  // Create effort joint interface
230 
231  joint_handle[0] = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
233  ej_interface_.registerHandle(joint_handle[0]);
234  }
235  else if(hardware_interface == "PositionJointInterface" || hardware_interface == "VelocityJointInterface"
236  || hardware_interface =="hardware_interface/PositionJointInterface" || hardware_interface == "hardware_interface/VelocityJointInterface" )
237  {
238 
239  ROS_INFO_STREAM_NAMED("mia_hw_sim", " Joint " << joint_names_[j]<< " POS-VEL" );
240  joint_handle.resize(2);
241 
242  List_joint_control_methods_[j].resize(2);
243 
244  // Create position joint interface
247 
248  joint_handle[0] = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
250  pj_interface_.registerHandle(joint_handle[0]);
251 
252  // Create velocity joint interface
253  hardware_interface::JointHandle joint_handle_vel;
255 
256  joint_handle[1] = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
258  vj_interface_.registerHandle(joint_handle[1]);
259  }
260  else
261  {
262  ROS_FATAL_STREAM_NAMED("default_robot_hw_sim","No matching hardware interface found for '"
263  << hardware_interface );
264  return false;
265  }
266 
267  //*
268  // Get the gazebo joint that corresponds to the robot joint.
269  gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]);
270 
271  if (!joint)
272  {
273  ROS_ERROR_STREAM("This robot has a joint named \"" << joint_names_[j]
274  << "\" which is not in the gazebo model.");
275  return false;
276  }
277  sim_joints_.push_back(joint); // Adds a new element at the end of the vector, after its current last element.
278 
279 
280  //*
281  // Get joint Limits
283  joint_limit_nh, urdf_model,
286 
287 
288 
289  if (List_joint_control_methods_[j][0] != EFFORT)
290  {
291  // Initialize the PID controller of the J_thumb_opp or other robot joints.
292  const ros::NodeHandle nh(model_nh, "/gazebo_ros_control/pid_gains/" +
293  joint_names_[j]);
294  if (pid_controllers_[j].init(nh, true))
295  {
296  switch (List_joint_control_methods_[j][0])
297  {
298  case POSITION:
300  if (j != joints_ii.j_thumb_opp)
301  {
302  ROS_WARN_STREAM_NAMED("mia_hw_sim","POSITION_PID control not effective for mia joints. Please use ROS controllers"
303  <<" to load pid gains");
304  }
305 
306  break;
307  case VELOCITY:
309  ROS_WARN_STREAM_NAMED("mia_hw_sim","VELOCITY_PID control not effective for mia joints. Please use ROS controllers"
310  <<" to load pid gains");
311  break;
312  }
313 
314  ROS_INFO_STREAM_NAMED("mia_hw_sim", " Get PID params !" );
315  }
316  else
317  {
318  // joint->SetParam("fmax") must be called if joint->SetAngle() or joint->SetParam("vel") are
319  // going to be called. joint->SetParam("fmax") must *not* be called if joint->SetForce() is
320  // going to be called.
321  #if GAZEBO_MAJOR_VERSION > 2
322  joint->SetParam("fmax", 0, joint_effort_limits_[j]); // fmax = maximum force (enum of the joint gazebo class)
323 
324  #else
325  joint->SetMaxForce(0, joint_effort_limits_[j]);
326  #endif
327  ROS_INFO_STREAM_NAMED("mia_hw_sim", " Set fmax !" );
328  }
329 
330 
331 
332 
333  }
334  }// endfor joints
335 
336  // Register interfaces
337  registerInterface(&js_interface_);
338  registerInterface(&ej_interface_);
339  registerInterface(&pj_interface_);
340  registerInterface(&vj_interface_);
341 
342  // Initialize simulated Mia hand variables values
344  j_index_flex_pos_Th = 0.1848; // Threshold to change the sign of the index joint in position control
345  j_index_flex_sign = 1; // initialize index position sign
346 
349 
350 
351  // Initialize the emergency stop code.
352  // TO BE DELETED IN THE NEXT VERSIONS
353  e_stop_active_ = false;
354  last_e_stop_active_ = false;
355 
356  return true;
357  }
358 
359 
361  // RegisterJointLimits:Register the limits of the joint specified by joint_name and joint_handle.
362  // The limits are retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also.
363  // Return the joint's type, lower position limit, upper position limit, and effort limit.
365  void MiaHWSim::registerJointLimits(const std::string& joint_name,
366  const std::vector<hardware_interface::JointHandle>& joint_handle,
367  const std::vector<ControlMethod> ctrl_method,
368  const ros::NodeHandle& joint_limit_nh,
369  const urdf::Model *const urdf_model,
370  int *const joint_type, double *const lower_limit,
371  double *const upper_limit, double *const effort_limit)
372  {
373  // Initialize variables
374  *joint_type = urdf::Joint::UNKNOWN;
375  *lower_limit = -std::numeric_limits<double>::max();
376  *upper_limit = std::numeric_limits<double>::max();
377  *effort_limit = std::numeric_limits<double>::max();
378 
379  joint_limits_interface::JointLimits limits;
380  bool has_limits = false;
381  joint_limits_interface::SoftJointLimits soft_limits;
382  bool has_soft_limits = false;
383 
384  // find limits
385  if (urdf_model != NULL)
386  {
387  //const boost::shared_ptr<const urdf::Joint> urdf_joint = urdf_model->getJoint(joint_name);
388  const urdf::JointConstSharedPtr urdf_joint = urdf_model->getJoint(joint_name);
389  if (urdf_joint != NULL)
390  {
391  *joint_type = urdf_joint->type;
392  // Get limits from the URDF file.
393  if (joint_limits_interface::getJointLimits(urdf_joint, limits))
394  has_limits = true;
395  if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits))
396  has_soft_limits = true;
397  }
398  }
399  // Get limits from the parameter server.
400  if (joint_limits_interface::getJointLimits(joint_name, joint_limit_nh, limits))
401  has_limits = true;
402 
403  if (!has_limits)
404  return;
405 
406  if (*joint_type == urdf::Joint::UNKNOWN)
407  {
408  // Infer the joint type.
409 
410  if (limits.has_position_limits)
411  {
412  *joint_type = urdf::Joint::REVOLUTE;
413  }
414  else
415  {
416  if (limits.angle_wraparound)
417  *joint_type = urdf::Joint::CONTINUOUS;
418  else
419  *joint_type = urdf::Joint::PRISMATIC;
420  }
421  }
422 
423  if (limits.has_position_limits)
424  {
425  *lower_limit = limits.min_position;
426  *upper_limit = limits.max_position;
427  }
428  if (limits.has_effort_limits)
429  *effort_limit = limits.max_effort;
430 
431  if (has_soft_limits)
432  {
433  for (unsigned int cm = 0; cm< ctrl_method.size(); cm++)
434  {
435  switch (ctrl_method[cm])
436  {
437  case EFFORT:
438  {
439  const joint_limits_interface::EffortJointSoftLimitsHandle
440  limits_handle(joint_handle[cm], limits, soft_limits);
441  ej_limits_interface_.registerHandle(limits_handle);
442  }
443  break;
444  case POSITION:
445  {
446  const joint_limits_interface::PositionJointSoftLimitsHandle
447  limits_handle(joint_handle[cm], limits, soft_limits);
448  pj_limits_interface_.registerHandle(limits_handle);
449  }
450  break;
451  case VELOCITY:
452  {
453  const joint_limits_interface::VelocityJointSoftLimitsHandle
454  limits_handle(joint_handle[cm], limits, soft_limits);
455  vj_limits_interface_.registerHandle(limits_handle);
456  }
457  break;
458  }
459  }
460  }
461  else
462  {
463  for (unsigned int cm = 0; cm< ctrl_method.size(); cm++)
464  {
465  switch (ctrl_method[cm])
466  {
467 
468  case EFFORT:
469  {
470  const joint_limits_interface::EffortJointSaturationHandle
471  sat_handle(joint_handle[cm], limits);
472  ej_sat_interface_.registerHandle(sat_handle);
473  }
474  break;
475  case POSITION:
476  {
477  const joint_limits_interface::PositionJointSaturationHandle
478  sat_handle(joint_handle[cm], limits);
479  pj_sat_interface_.registerHandle(sat_handle);
480  }
481  break;
482  case VELOCITY:
483  {
484  const joint_limits_interface::VelocityJointSaturationHandle
485  sat_handle(joint_handle[cm], limits);
486  vj_sat_interface_.registerHandle(sat_handle);
487  }
488  break;
489 
490  }//endswicth
491  }
492 
493  }//endifelse
494 
495  }//end registerlimits function
496 
497 
499  //readSim: Read state data from simulated robot hardware such as joint position, velocities,effort </summury>
501  void MiaHWSim::readSim(ros::Time time, ros::Duration period)
502  {
503  for(unsigned int j=0; j < n_dof_; j++)
504  {
505  double position;
506  double velocity;
507  double effort;
508 
509  //*
510  // 1) Read position
511  if (joint_types_[j] == urdf::Joint::PRISMATIC)
512  {
513  #if GAZEBO_MAJOR_VERSION >= 9
514  position = sim_joints_[j]->Position (0); // used with Gazebo 9
515  #else
516 
517  position = sim_joints_[j]->GetAngle(0).Radian(); //Deprecated used for Gazebo 7.1.0,
518  #endif
519 
520  }
521  else
522  {
523  #if GAZEBO_MAJOR_VERSION >= 9
524  position = joint_position_[j] + angles::shortest_angular_distance(joint_position_[j],
525  sim_joints_[j]->Position (0));
526 
527  #else
528  position = joint_position_[j] + angles::shortest_angular_distance(joint_position_[j],
529  sim_joints_[j]->GetAngle(0).Radian());
530  #endif
531  }
532 
533  //*
534  // 2) Read velocity
537  {
538  velocity = (position - joint_position_[j]) / period.toSec(); // rad/sec estimated for mia_hand
539  }
540  else
541  {
542  velocity = sim_joints_[j]->GetVelocity(0); // return wrong values
543  }
544  //*
545  // 3) Read effort
546  effort = sim_joints_[j]->GetForce((unsigned int)(0));
547 
548  //*
549  // 4) Publish joint states
550  if (j == joints_ii.j_index_flex)
551  {
552  // Read velocity with the right sign for the index
553  velocity = ((position * j_index_flex_sign) - joint_position_[j]) / period.toSec(); // rad/sec estimated
554 
555  joint_position_[j] = (position * j_index_flex_sign);
556  joint_velocity_[j] = velocity;
557  joint_effort_[j] = effort * j_index_flex_sign;
558  }
559  else
560  {
561 
562  joint_position_[j] = position;
563  joint_velocity_[j] = velocity;
564  joint_effort_[j] = effort;
565  }
566 
567  } // end for joint
568 
569  }// end readSim function
570 
572  //writeSim: Write commands to the simulated robot hardware. such as joint position, velocities,
573  // effort commands
575  void MiaHWSim::writeSim(ros::Time time, ros::Duration period)
576  {
577 
578  /* Handle the initialization of the last commands when this function
579  has been called for the first time */
580  if(write_counter == 0)
581  {
582  // Initialize variables used to select the control methods for the joints
583  for(unsigned int j=0; j < n_dof_; j++)
584  {
585  // for the first iteration initialize the last command and the last joint position status
589 
591  }
592  write_counter++;
593  }
594 
595  // If the E-stop is active, joints controlled by position commands will maintain their positions.
596  if (e_stop_active_)
597  {
598  if (!last_e_stop_active_)
599  {
601  last_e_stop_active_ = true;
602  }
604  }
605  else
606  {
607  last_e_stop_active_ = false;
608  }
609 
610  // Enforce limits
611  ej_sat_interface_.enforceLimits(period);
612  ej_limits_interface_.enforceLimits(period);
613  pj_sat_interface_.enforceLimits(period);
614  pj_limits_interface_.enforceLimits(period);
615  vj_sat_interface_.enforceLimits(period);
616  vj_limits_interface_.enforceLimits(period);
617 
618  // Set the max difference position that is allowed between middle, ring and middle finger
619  const double MaxDiff_mrl = 0.1; // rad
620 
621  for(unsigned int j=0; j < n_dof_; j++)
622  {
623 
624  /* Skip write action for j_mrl_2 and j_mrl_3 joints since they
625  have to follow j_mrl_flex joint */
626  if(j == joints_ii.j_mrl_2 || j == joints_ii.j_mrl_3 || j == joints_ii.j_thumb_opp)
627  {continue;}
628 
635 
636  // Save the last recevived command for the next function call
641 
642  //ROS_INFO_STREAM("Joint " <<j << " "<< joint_control_methods_[j]);
643 
644  // Control the joint based on the selected control method
645  switch (joint_control_methods_[j])
646  {
647  case EFFORT:
648  {
649  const double effort = e_stop_active_ ? 0 : joint_effort_command_[j];
650 
651  if (j == joints_ii.j_mrl_flex) // then send the command also to jmrl_2 and j_mrl_3
652  {
653  // Check if ring little fingers get stack and evaluate the the middle position accordingly
655  if (isStack)
656  {
657  double target_pos = jMiddle_StackTarget_position; // stack --> Get fixed position
658  sim_joints_[j]->SetPosition(0, target_pos); // command the middle joint in to stop in position
659  }
660  else
661  {
662  sim_joints_[j]->SetForce(0, effort); // unstack --> normal behaviour
663  }
664 
665 
666  if(!mrl_united) // if the middle ring little joints are not physically united in the URDF then...
667  {
668  // ring and little has to mirror the target position of the middle
669  sim_joints_[joints_ii.j_mrl_2]->SetPosition(0, joint_position_[j]);
670  sim_joints_[joints_ii.j_mrl_3]->SetPosition(0, joint_position_[j]);
671  }
672  }
673  else if (j == joints_ii.j_index_flex) // then menage also the thumb position control
674  {
675  // Get the position to send to the j_thumb_opp
676  const double j_thumb_opp_effort_control = GetThumbOppPosition(period);
678 
679  if(*j_index_flex_pos ==0 )
680  {
681  j_index_flex_sign = 1;
682  sim_joints_[j]->SetForce(0, effort);
683  }
684 
685  // this condition prevents that the index get stuck when it is started at pos = 0
686  else if (*j_index_flex_pos < 0 && *j_index_flex_pos >= -0.0005 && j_index_flex_sign == 1)
687  {
688  j_index_flex_sign = -1;
689  sim_joints_[joints_ii.j_index_flex]->SetPosition(0, -(-0.0005));
690  }
691 
692  else if (*j_index_flex_pos < 0 && *j_index_flex_pos < -0.0005 && j_index_flex_sign == 1)
693  {
694  j_index_flex_sign = -1;
695  sim_joints_[joints_ii.j_index_flex]->SetPosition(0, - *j_index_flex_pos);
696  }
697 
698  else if (*j_index_flex_pos < 0 && j_index_flex_sign == -1 )
699  {
700  j_index_flex_sign = -1;
701  sim_joints_[j]->SetForce(0, -joint_effort_command_[j]);
702 
703  }
704 
705  else if (*j_index_flex_pos > 0 && j_index_flex_sign == -1 )
706  {
707  j_index_flex_sign = 1;
709  }
710  else if (*j_index_flex_pos > 0 && j_index_flex_sign == 1 )
711  {
712 
713  j_index_flex_sign = 1;
714  sim_joints_[j]->SetForce(0, effort);
715  }
716 
717  }
718 
719  /* For all the other joints of the simulated hardware (i.e the Mia
720  thumb flexion joint or joints of a robot arm ) behave in the
721  standard way */
722  else
723  {
724  sim_joints_[j]->SetForce(0, effort);
725  }
726 
727  }
728  break;
729 
730  case POSITION:
731  {
732 
733  #if GAZEBO_MAJOR_VERSION >= 4
734 
735  if (j == joints_ii.j_mrl_flex) // then send the command also to jmrl_2 and j_mrl_3
736  {
737  // Check if ring little fingers get stack and evaluate the the middle position accordingly
738  double flex_direction = joint_position_command_[j] - joint_position_[j];
740  if (isStack)
741  {
742  double target_pos = jMiddle_StackTarget_position; // stack --> fixed position
743  // command the middle joint in to stop in position
744  sim_joints_[j]->SetPosition(0, target_pos);
745  }
746  else
747  {
748  sim_joints_[j]->SetPosition(0, joint_position_command_[j]); // unstack --> normal behaviour
749  }
750 
751  if(!mrl_united) // if the middle ring little joints are not physically united in the URDF
752  {
753 
754  // ring and little has to mirror the position of the middle
755  sim_joints_[joints_ii.j_mrl_2]->SetPosition(0, joint_position_[j]);
756  sim_joints_[joints_ii.j_mrl_3]->SetPosition(0, joint_position_[j]);
757 
758  }
759 
760  }
761  else if (j == joints_ii.j_index_flex) // then menage also the thumb position control
762  {
763  // Get the position to send to the j_thumb_opp
764  const double j_thumb_opp_effort_control = GetThumbOppPosition(period);
766 
767  //ROS_INFO_STREAM_NAMED("mia_hw_sim", "PI: " << joint_position_command_[j] );
769  {
770 
772  j_index_flex_sign = -1;
773 
774  }
775  else
776  {
778  j_index_flex_sign = 1;
779  }
780 
781  }
782  else if (j == joints_ii.j_thumb_flex )
783  {
785  {
786  sim_joints_[j]->SetPosition(0, joint_position_command_[j]+0.01);
787  }
788  else
789  {
790  sim_joints_[j]->SetPosition(0, joint_position_command_[j]);
791  }
792 
793  }
794 
795  /* For all the other joints of the simulated hardware (i.e joints of
796  a robot arm ) behave in the standard way */
797  else
798  {sim_joints_[j]->SetPosition(0, joint_position_command_[j]);}
799 
800  #else
801  if (j == joints_ii.j_mrl_flex) // then send the command also to jmrl_2 and j_mrl_3
802 
803  {
804 
805  sim_joints_[j]->SetAngle(0, joint_position_command_[j]);
806  if(!mrl_united) // if the middle ring little joints are not physically united in the URDF
807  {
808  // ring and little has to mirror the position of the middle
809  sim_joints_[joints_ii.j_mrl_2]->SetAngle(0, joint_position_[j]);
810  sim_joints_[joints_ii.j_mrl_3]->SetAngle(0, joint_position_[j]);
811  }
812 
813  }
814  else if (j == joints_ii.j_index_flex) // then menage also the thumb position control
815  {
816  // Get the position to send to the j_thumb_opp
817  const double j_thumb_opp_effort_control = GetThumbOppPosition(period);
819 
820 
822  {
823 
825  j_index_flex_sign = -1;
826 
827  }
828  else
829  {
831  j_index_flex_sign = 1;
832  }
833 
834  }
835  else if (j == joints_ii.j_thumb_flex )
836  {
838  {
839  sim_joints_[j]->SetAngle(0, joint_position_command_[j]+0.01);
840  }
841  else
842  {
843  sim_joints_[j]->SetAngle(0, joint_position_command_[j]);
844  }
845 
846  }
847 
848  /* For all the other joints of the simulated hardware (i.e joints of
849  a robot arm ) behave in the standard way */
850  else
851  {sim_joints_[j]->SetAngle(0, joint_position_command_[j]);}
852 
853  #endif
854  }
855  break;
856 
857  case POSITION_PID:
858  {
859 
861  {continue;} // control type not implemented for mia hand joints
862 
863  double error;
864  switch (joint_types_[j])
865  {
866  case urdf::Joint::REVOLUTE:
867  angles::shortest_angular_distance_with_limits(joint_position_[j],
871  error);
872  break;
873  case urdf::Joint::CONTINUOUS:
874  error = angles::shortest_angular_distance(joint_position_[j],
876  break;
877  default:
879  }
880 
881  const double effort_limit = joint_effort_limits_[j];
882  const double effort = clamp(pid_controllers_[j].computeCommand(error, period),
883  -effort_limit, effort_limit);
884  sim_joints_[j]->SetForce(0, effort);
885 
886  }
887  break;
888 
889  case VELOCITY:
890  {
891 
892  const double velocity_control = e_stop_active_ ? 0 : joint_velocity_command_[j];
893 
894  // since SetParam("vel", 0, velocity_control); seems not to work properly we bypassed it simulating and instantaneus velocity
895  //using position control and the simulation time step
896  double delta = velocity_control * period.toSec();
897  double target_pos;
898 
899  #if GAZEBO_MAJOR_VERSION >= 4
900 
901  if (j == joints_ii.j_mrl_flex) // then send the command also to jmrl_2 and j_mrl_3
902  {
903 
904  // Check if ring little fingers get stack and evaluate the the middle position accordingly
906  if (isStack)
907  {
908  target_pos = jMiddle_StackTarget_position; // stack --> fixed position
909  }
910  else
911  {
912  target_pos = joint_position_[j] + delta; // unstack --> normal behaviour
913  }
914 
915  // command the middle joint
916  sim_joints_[j]->SetPosition(0, target_pos);
917 
918  if(!mrl_united) // if the middle ring little joints are not physically united in the URDF then...
919  {
920  // ring and little has to mirror the position of the middle
921  sim_joints_[joints_ii.j_mrl_2]->SetPosition(0, joint_position_[j]);
922  sim_joints_[joints_ii.j_mrl_3]->SetPosition(0, joint_position_[j]);
923 
924  }
925  }
926  else if (j == joints_ii.j_index_flex) // then menage also the thumb position control
927  {
928  // Get the position to send to the j_thumb_opp
929  const double j_thumb_opp_effort_control = GetThumbOppPosition(period);
931 
932  // index control
933  if(*j_index_flex_pos ==0 )
934  {
935  j_index_flex_sign = 1;
936  target_pos = joint_position_[j] + delta;
937  sim_joints_[j]->SetPosition(0, target_pos);
938  }
939 
940  // This condition avoids that the index get stuck when it is started at pos = 0
941  else if (*j_index_flex_pos < 0 && *j_index_flex_pos >= -0.0005 && j_index_flex_sign == 1)
942  {
943  j_index_flex_sign = -1;
944  sim_joints_[joints_ii.j_index_flex]->SetPosition(0, - (-0.0005));
945  }
946 
947  else if (*j_index_flex_pos < 0 && *j_index_flex_pos < -0.0005 && j_index_flex_sign == 1)
948  {
949  j_index_flex_sign = -1;
950  sim_joints_[joints_ii.j_index_flex]->SetPosition(0, - *j_index_flex_pos);
951  }
952 
953  else if (*j_index_flex_pos < 0 && j_index_flex_sign == -1 )
954  {
955  j_index_flex_sign = -1;
956  target_pos = -joint_position_[j] - delta;
957  sim_joints_[j]->SetPosition(0, target_pos);
958  }
959 
960  else if (*j_index_flex_pos > 0 && j_index_flex_sign == -1 )
961  {
962  j_index_flex_sign = 1;
964  }
965 
966  else if (*j_index_flex_pos > 0 && j_index_flex_sign == 1 )
967  {
968  j_index_flex_sign = 1;
969  target_pos = joint_position_[j] + delta;
970  sim_joints_[j]->SetPosition(0, target_pos);
971  }
972 
973  }
974  else if (j == joints_ii.j_thumb_flex )
975  {
976  target_pos = joint_position_[j] + delta;
977  sim_joints_[j]->SetPosition(0, target_pos);
978  }
979 
980  /* For all the other joints of the simulated hardware (i.e joints of
981  a robot arm ) behave in the standard way */
982  else
983  {
984  sim_joints_[j]->SetParam("vel", 0, e_stop_active_ ? 0 : joint_velocity_command_[j]);
985  }
986 
987  #elif GAZEBO_MAJOR_VERSION <= 2
988 
989  if (j == joints_ii.j_mrl_flex) // then send the command also to jmrl_2 and j_mrl_3
990  {
991  target_pos = joint_position_[j] + delta;
992  sim_joints_[j]->SetAngle(0, target_pos);
993 
994  if(!mrl_united) // if the middle ring little joints are not physically united in the URDF
995  {
996  // ring and little has to mirror the position of the middle
997  sim_joints_[joints_ii.j_mrl_2]->SetAngle(0, joint_position_[j]);
998  sim_joints_[joints_ii.j_mrl_3]->SetAngle(0, joint_position_[j]);
999  }
1000  }
1001  else if (j == joints_ii.j_index_flex) // then menage also the thumb position control
1002  {
1003 
1004 
1005  // Get the position to send to the j_thumb_opp
1006  const double j_thumb_opp_effort_control = GetThumbOppPosition(period);
1008 
1009 
1010  // index control
1011  if(*j_index_flex_pos ==0 )
1012  {
1013  j_index_flex_sign = 1;
1014  target_pos = joint_position_[j] + delta;
1015  sim_joints_[j]->SetAngle(0, target_pos);
1016 
1017 
1018  }
1019  else if (*j_index_flex_pos < 0 && j_index_flex_sign == 1)
1020  {
1021  j_index_flex_sign = -1;
1023 
1024  }
1025 
1026 
1027  else if (*j_index_flex_pos < 0 && j_index_flex_sign == -1 )
1028  {
1029  j_index_flex_sign = -1;
1030  target_pos = -joint_position_[j] - delta;
1031  sim_joints_[j]->SetAngle(0, target_pos);
1032 
1033 
1034  }
1035 
1036  else if (*j_index_flex_pos > 0 && j_index_flex_sign == -1 )
1037  {
1038  j_index_flex_sign = 1;
1040  }
1041  else if (*j_index_flex_pos > 0 && j_index_flex_sign == 1 )
1042  {
1043 
1044  j_index_flex_sign = 1;
1045  target_pos = joint_position_[j] + delta;
1046  sim_joints_[j]->SetAngle(0, target_pos);
1047 
1048 
1049  }
1050  }
1051  else if (j == joints_ii.j_thumb_flex )
1052  {
1053  target_pos = joint_position_[j] + delta;
1054  sim_joints_[j]->SetAngle(0, target_pos);
1055  }
1056 
1057  /* For all the other joints of the simulated hardware (i.e joints of
1058  a robot arm ) behave in the standard way */
1059  else
1060  {
1061  sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_velocity_command_[j]);
1062  }
1063  #else
1064 
1065  if (j == joints_ii.j_mrl_flex) // then send the command also to jmrl_2 and j_mrl_3
1066  {
1067  target_pos = joint_position_[j] + delta;
1068  sim_joints_[j]->SetAngle(0, target_pos);
1069 
1070  if(!mrl_united) // if the middle ring little joints are not physically united in the URDF
1071  {
1072  // ring and little has to mirror the position of the middle
1073  sim_joints_[joints_ii.j_mrl_2]->SetAngle(0, joint_position_[j]);
1074  sim_joints_[joints_ii.j_mrl_3]->SetAngle(0, joint_position_[j]);
1075  }
1076  }
1077  else if (j == joints_ii.j_index_flex) // then menage also the thumb position control
1078  {
1079  // Get the position to send to the j_thumb_opp
1080  const double j_thumb_opp_effort_control = GetThumbOppPosition(period);
1082 
1083  // index control
1084  if(*j_index_flex_pos ==0 )
1085  {
1086  j_index_flex_sign = 1;
1087  target_pos = joint_position_[j] + delta;
1088  sim_joints_[j]->SetAngle(0, target_pos);
1089  }
1090 
1091  else if (*j_index_flex_pos < 0 && j_index_flex_sign == 1)
1092  {
1093  j_index_flex_sign = -1;
1095  }
1096 
1097  else if (*j_index_flex_pos < 0 && j_index_flex_sign == -1 )
1098  {
1099  j_index_flex_sign = -1;
1100  target_pos = -joint_position_[j] - delta;
1101  sim_joints_[j]->SetAngle(0, target_pos);
1102  }
1103 
1104  else if (*j_index_flex_pos > 0 && j_index_flex_sign == -1 )
1105  {
1106  j_index_flex_sign = 1;
1108  }
1109 
1110  else if (*j_index_flex_pos > 0 && j_index_flex_sign == 1 )
1111  {
1112  j_index_flex_sign = 1;
1113  target_pos = joint_position_[j] + delta;
1114  sim_joints_[j]->SetAngle(0, target_pos);
1115  }
1116  }
1117  else if (j == joints_ii.j_thumb_flex )
1118  {
1119  target_pos = joint_position_[j] + delta;
1120  sim_joints_[j]->SetAngle(0, target_pos);
1121  }
1122 
1123  /* For all the other joints of the simulated hardware (i.e joints of
1124  a robot arm ) behave in the standard way */
1125  else
1126  {
1127  sim_joints_[j]->SetParam("vel", 0, e_stop_active_ ? 0 : joint_velocity_command_[j]);
1128  }
1129 
1130  #endif
1131  }
1132  break;
1133 
1134  case VELOCITY_PID:
1135  {
1137  {continue;} // control type not implemented for mia hand joints
1138 
1139 
1140  double error;
1141  if (e_stop_active_)
1142  error = -joint_velocity_[j];
1143  else
1144  error = joint_velocity_command_[j] - joint_velocity_[j];
1145 
1146  const double effort_limit = joint_effort_limits_[j];
1147  const double effort = clamp(pid_controllers_[j].computeCommand(error, period),
1148  -effort_limit, effort_limit);
1149  sim_joints_[j]->SetForce(0, effort);
1150  }
1151  break;
1152 
1153  } // switch
1154  } // for joint
1155  } // end function
1156 
1157 
1159  //SelectCtrMethod: Select the method to use to control a joint. </summury>
1162  enum MiaHWSim::ControlMethod last_joint_control_methods_,
1163  const double last_joint_velocity_command_,
1164  const double joint_velocity_command_,
1165  const double last_joint_position_command_,
1166  const double joint_position_command_)
1167  {
1168 
1169  if(Default_joint_control_methods_ == EFFORT)
1170  return EFFORT;
1171  else
1172  {
1173  if( joint_position_command_ != last_joint_position_command_) // is the only command that if it changes it is sure that some one modified the requested position
1174  return POSITION;
1176  return VELOCITY;
1177  else // default
1179  }
1180  }
1181 
1183  //GetThumbOppPosition: Get the target position of the Mia thumb_opp joint based on the position of
1184  //the simulated Mia index actual position.
1186  double MiaHWSim::GetThumbOppPosition(ros::Duration period)
1187  {
1189 
1190  // Use a pid to calculate the effort for the thumb
1191  double error;
1192 
1193  angles::shortest_angular_distance_with_limits(joint_position_[joints_ii.j_thumb_opp], jThOpp_Target_position,
1196  error);
1197 
1198  const double effort_limit = joint_effort_limits_[joints_ii.j_thumb_opp];
1199  const double effort = clamp(pid_controllers_[joints_ii.j_thumb_opp].computeCommand(error, period),
1200  -effort_limit, effort_limit);
1201  return effort;
1202  }
1203 
1204 
1206  //GetMiddlePosition: Get the position of the Mia mrl1 joint based in order to avoid that a max
1207  // distance between mrl1 mrl2 and mrl3 can occur. This is usefull to simulate situation in which
1208  // the little or ring finger of the simulated Mia hand get stack.
1209  // flex direction can be teh velocity control received, the effort control etc </summury>
1211  bool MiaHWSim::GetMiddlePosition(double joint_position_m, double joint_position_r, double joint_position_l, double flex_direction)
1212  {
1213  // Initialize variables
1214  const double MaxDist = 0.05; // rad max distance that can be accepted between the middle, ring and little fingers
1215  double isStack = false;
1216 
1217 
1218  // Simulate the stack of the ring and/or little finger
1219  double diff_mr = joint_position_m - joint_position_r;
1220  double diff_ml = joint_position_m - joint_position_l;
1221 
1222  if (diff_mr >= MaxDist & flex_direction > 0 ) // If m is more flexed than ring --> m can only extend
1223  {
1224  jMiddle_StackTarget_position = joint_position_r + (MaxDist+0.01);
1225  isStack = true;
1226  }
1227 
1228  else if (diff_ml >= MaxDist & flex_direction > 0 ) // If m is more flexed than little --> m can only extend
1229  {
1230  jMiddle_StackTarget_position = joint_position_l + (MaxDist+0.01);
1231  isStack = true;
1232  }
1233 
1234  else if (diff_mr <= -MaxDist & flex_direction < 0 ) // If m is more extended than ring --> m can only flex
1235  {
1236  jMiddle_StackTarget_position = joint_position_r - (MaxDist+0.01);
1237  isStack = true;
1238  }
1239 
1240  else if (diff_ml <= -MaxDist & flex_direction < 0 ) // If m is more extended than ring --> m can only flex
1241  {
1242  jMiddle_StackTarget_position = joint_position_l - (MaxDist+0.01);
1243  isStack = true;
1244  }
1245 
1246  else
1247  {
1248  isStack = false;
1249  }
1250 
1251  return isStack;
1252  }
1253 
1255  //eStopActive: Set the simulated robot's emergency stop state. The default implementation of
1256  // this function does nothing.
1258  void MiaHWSim::eStopActive(const bool active)
1259  {
1260  e_stop_active_ = active;
1261  }
1262 
1263 }//end namespace mia
1264 
1265 // the pluginlib macros that allow us to register classes as plugins.
1266  PLUGINLIB_EXPORT_CLASS(mia::MiaHWSim, gazebo_ros_control::RobotHWSim)
1267 // PLUGINLIB_EXPORT_CLASS(namespace::PluginClassName, namespace::BaseClassName)1
mia::MiaHWSim::SelectCtrMethod
enum ControlMethod SelectCtrMethod(enum MiaHWSim::ControlMethod Default_joint_control_methods_, enum ControlMethod last_joint_control_methods_, const double last_joint_velocity_command_, const double joint_velocity_command_, const double last_joint_position_command_, const double joint_position_command_)
Select the control method to move the simulated hardware.
Definition: Mia_hw_sim.cpp:1161
Mia_hw_sim.h
mia::MiaHWSim::vj_interface_
hardware_interface::VelocityJointInterface vj_interface_
Interface for simulated joint velocity commands.
Definition: Mia_hw_sim.h:204
mia::MiaHWSim::n_dof_
unsigned int n_dof_
Number of degree of freedom of the simulated hardware.
Definition: Mia_hw_sim.h:197
mia::MiaHWSim::VELOCITY
@ VELOCITY
Definition: Mia_hw_sim.h:117
mia::MiaHWSim::joints_ii
Joint_index_mapping joints_ii
Joint_index_mapping struct.
Definition: Mia_hw_sim.h:195
mia::MiaHWSim::ej_limits_interface_
joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_
Interface for simulated joint effort soft limit.
Definition: Mia_hw_sim.h:207
mia::MiaHWSim::jMiddle_StackTarget_position
double jMiddle_StackTarget_position
Target position of the Mia simulated middle flexion joint when ring and/or little get stuck.
Definition: Mia_hw_sim.h:241
mia::MiaHWSim::jThOpp_Target_position
double jThOpp_Target_position
Target position of the Mia simulated thumb opposition joint evaluated by this class.
Definition: Mia_hw_sim.h:240
mia::MiaHWSim::pid_controllers_
std::vector< control_toolbox::Pid > pid_controllers_
Definition: Mia_hw_sim.h:234
mia::Joint_index_mapping::j_index_flex
int j_index_flex
Index number of the index_fle joint.
Definition: Mia_hw_sim.h:62
mia::MiaHWSim::pj_limits_interface_
joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_
Interface for simulated joint position sodt limit.
Definition: Mia_hw_sim.h:209
mia::MiaHWSim::joint_lower_limits_
std::vector< double > joint_lower_limits_
Lower position limits of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:216
mia::MiaHWSim::EFFORT
@ EFFORT
Definition: Mia_hw_sim.h:117
mia::MiaHWSim::writeSim
virtual void writeSim(ros::Time time, ros::Duration period)
Send commands to the simulated robot hardware.
Definition: Mia_hw_sim.cpp:575
mia::MiaHWSim::ej_sat_interface_
joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_
Interface for simulated joint effort saturation limit.
Definition: Mia_hw_sim.h:206
mia::MiaHWSim::POSITION_PID
@ POSITION_PID
Definition: Mia_hw_sim.h:117
mia::MiaHWSim::POSITION
@ POSITION
Definition: Mia_hw_sim.h:117
mia::Joint_index_mapping::j_thumb_opp
int j_thumb_opp
Index number of the thumb_opp joint.
Definition: Mia_hw_sim.h:60
mia::MiaHWSim::VELOCITY_PID
@ VELOCITY_PID
Definition: Mia_hw_sim.h:117
mia::MiaHWSim::List_joint_control_methods_
std::vector< std::vector< ControlMethod > > List_joint_control_methods_
List of control method available for the joints of the simulated hardware.
Definition: Mia_hw_sim.h:232
mia::Joint_index_mapping::j_mrl_3
int j_mrl_3
Index number of the mrl_3 (little) joint.
Definition: Mia_hw_sim.h:65
mia::MiaHWSim::last_joint_effort_command_
std::vector< double > last_joint_effort_command_
Last effort command of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:224
mia::MiaHWSim::ej_interface_
hardware_interface::EffortJointInterface ej_interface_
Interface for simulated joint effort commands.
Definition: Mia_hw_sim.h:202
mia::MiaHWSim::GetThumbOppPosition
double GetThumbOppPosition(ros::Duration period)
Evaluate the position of the Mia thumb opposition joint.
Definition: Mia_hw_sim.cpp:1186
mia::MiaHWSim::j_index_flex_pos
double * j_index_flex_pos
Pointer to the position status of Mia simulated index flexion joint.
Definition: Mia_hw_sim.h:237
mia::MiaHWSim::vj_sat_interface_
joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_
Interface for simulated joint velocity saturation limit.
Definition: Mia_hw_sim.h:210
mia::MiaHWSim::pj_sat_interface_
joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_
Interface for simulated joint position saturation limit.
Definition: Mia_hw_sim.h:208
mia::MiaHWSim::last_joint_position_command_
std::vector< double > last_joint_position_command_
Last position command of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:226
mia::MiaHWSim::joint_effort_command_
std::vector< double > joint_effort_command_
Actual effort command of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:223
mia::MiaHWSim::initSim
virtual bool initSim(const std::string &robot_namespace, ros::NodeHandle model_nh, gazebo::physics::ModelPtr parent_model, const urdf::Model *const urdf_model, std::vector< transmission_interface::TransmissionInfo > transmissions)
Initialize the simulated robot hardware.
Definition: Mia_hw_sim.cpp:31
mia::MiaHWSim::registerJointLimits
void registerJointLimits(const std::string &joint_name, const std::vector< hardware_interface::JointHandle > &joint_handle, const std::vector< ControlMethod > ctrl_method, const ros::NodeHandle &joint_limit_nh, const urdf::Model *const urdf_model, int *const joint_type, double *const lower_limit, double *const upper_limit, double *const effort_limit)
Register the joint limits of the simulated hardware.
Definition: Mia_hw_sim.cpp:365
mia::MiaHWSim::MyTh_opp_passiveJoint
mia_hand::thumb_opp_passive_joint MyTh_opp_passiveJoint
Class used to evaluate the target position of the Mia simulated thumb opposition joint.
Definition: Mia_hw_sim.h:251
mia::Joint_index_mapping::j_mrl_2
int j_mrl_2
Index number of the mrl_2 (ring) joint.
Definition: Mia_hw_sim.h:64
mia::MiaHWSim::write_counter
unsigned int write_counter
Counter of the write method.
Definition: Mia_hw_sim.h:198
mia::MiaHWSim::joint_upper_limits_
std::vector< double > joint_upper_limits_
Upper position limits of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:217
mia::MiaHWSim::joint_control_methods_
std::vector< ControlMethod > joint_control_methods_
Actual control method for the joints of the simulated hardware.
Definition: Mia_hw_sim.h:231
mia::MiaHWSim::joint_position_command_
std::vector< double > joint_position_command_
Actual position command of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:225
mia::MiaHWSim::joint_position_
std::vector< double > joint_position_
Actual position state of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:220
mia::MiaHWSim::GetMiddlePosition
bool GetMiddlePosition(double joint_position_m, double joint_position_r, double joint_position_l, double flex_direction)
Evaluate if any of the Mia simulated middle ring or little flexion joint is stuck.
Definition: Mia_hw_sim.cpp:1211
mia::MiaHWSim::last_joint_control_methods_
std::vector< ControlMethod > last_joint_control_methods_
Last control method used for the joints of the simulated hardware.
Definition: Mia_hw_sim.h:229
mia::MiaHWSim::joint_velocity_
std::vector< double > joint_velocity_
Actual velocity state of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:221
mia::MiaHWSim::joint_names_
std::vector< std::string > joint_names_
Name of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:214
mia::MiaHWSim::ControlMethod
ControlMethod
Joints control methods.
Definition: Mia_hw_sim.h:117
mia::MiaHWSim::js_interface_
hardware_interface::JointStateInterface js_interface_
Interface for simulated joint state.
Definition: Mia_hw_sim.h:201
mia::MiaHWSim::joint_effort_
std::vector< double > joint_effort_
Actual effort state of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:222
mia::MiaHWSim::last_joint_velocity_command_
std::vector< double > last_joint_velocity_command_
Last velocity command of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:228
mia::MiaHWSim::joint_effort_limits_
std::vector< double > joint_effort_limits_
Effort upper limits of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:218
mia::MiaHWSim::sim_joints_
std::vector< gazebo::physics::JointPtr > sim_joints_
Gazebo joint pointer.
Definition: Mia_hw_sim.h:254
mia::MiaHWSim::joint_types_
std::vector< int > joint_types_
Type of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:215
mia::MiaHWSim::vj_limits_interface_
joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_
Interface for simulated joint velocity soft limit.
Definition: Mia_hw_sim.h:211
mia::MiaHWSim::mrl_united
bool mrl_united
If True then middle ring little fingers are physical connected in the URDF, False if they ate indepen...
Definition: Mia_hw_sim.h:242
mia::MiaHWSim::eStopActive
virtual void eStopActive(const bool active)
Unused.
Definition: Mia_hw_sim.cpp:1258
mia
Definition: Mia_hw_sim.h:41
mia::MiaHWSim::readSim
virtual void readSim(ros::Time time, ros::Duration period)
Read state data from the simulated robot hardware.
Definition: Mia_hw_sim.cpp:501
mia::Joint_index_mapping::j_thumb_flex
int j_thumb_flex
Index number of the thumb_fle joint.
Definition: Mia_hw_sim.h:61
mia::MiaHWSim::j_index_flex_sign
int j_index_flex_sign
Current sign of the Mia simulated index flexion joint.
Definition: Mia_hw_sim.h:239
mia::MiaHWSim::e_stop_active_
bool e_stop_active_
Definition: Mia_hw_sim.h:257
mia::MiaHWSim::joint_velocity_command_
std::vector< double > joint_velocity_command_
Actual velocity command of the joints of the simulated hardware.
Definition: Mia_hw_sim.h:227
mia::MiaHWSim
Class hardware sim interface of the simulated Mia hand.
Definition: Mia_hw_sim.h:71
mia::MiaHWSim::j_index_flex_pos_Th
double j_index_flex_pos_Th
Threshold used for the simulated index flexion position control to switch its sign.
Definition: Mia_hw_sim.h:238
mia::MiaHWSim::last_e_stop_active_
bool last_e_stop_active_
Unused.
Definition: Mia_hw_sim.h:257
mia::MiaHWSim::pj_interface_
hardware_interface::PositionJointInterface pj_interface_
Interface for simulated joint position commands.
Definition: Mia_hw_sim.h:203
mia::Joint_index_mapping::j_mrl_flex
int j_mrl_flex
Index number of the mrl_fle (medium) joint.
Definition: Mia_hw_sim.h:63