mia_hand_description  rel 1.0.0
mia_thumb_opp_passivejoints.cpp
Go to the documentation of this file.
1 
8 
9 
10 namespace mia_hand
11 {
12 
15  {
16 
17  }
18 
19  void thumb_opp_passive_joint::init(const bool LoadURDFInfo_ )
20  {
21  j_index_name ="j_index_fle"; // default
22  j_thumb_name ="j_thumb_opp"; // default
23  robot_description_ = "robot_description"; // default
24 
25  if(LoadURDFInfo_) { bool temp = LoadURDFInfo();}
26  else
27  {
28  ThMinPos=0;
29  ThMaxPos= 0;
30  }
31 
32  }
33 
34 
36 
38  double thumb_opp_passive_joint::GetThumbOppPosition(double j_index_flex_pos)
39  {
40  // Implementation of g3(delta) according with Mia trasmission and empirical test
41  double Empirical_Scale_j_thumb_opp_pos = 0.95238;
42  double Empirical_Offset_j_thumb_opp_pos = -0.0052884;
43  double jThOpp_Target_position_temp = Empirical_Scale_j_thumb_opp_pos * (j_index_flex_pos) + Empirical_Offset_j_thumb_opp_pos;
44  double jThOpp_Target_position;
45 
46  if (jThOpp_Target_position_temp <= ThMinPos)
47  jThOpp_Target_position = ThMinPos + 0.02;
48 
49  else if (jThOpp_Target_position_temp >= ThMaxPos)
50  jThOpp_Target_position = ThMaxPos - 0.02;
51 
52  else
53  jThOpp_Target_position = jThOpp_Target_position_temp;
54 
55  return jThOpp_Target_position;
56  }
57 
58 
61  {
62  bool result = false;
63 
64  const std::string urdf_string = getURDF(robot_description_);
65 
66  // get URDF model
67  urdf::Model urdf_model;
68  const urdf::Model *const urdf_model_ptr = urdf_model.initString(urdf_string) ? &urdf_model : NULL;
69 
70  const urdf::JointConstSharedPtr urdf_joint = urdf_model_ptr->getJoint(j_thumb_name);
71 
72  joint_limits_interface::JointLimits limits;
73 
74  if (urdf_joint != NULL)
75  {
76 
77  // Get limits from the URDF file.
78  if (joint_limits_interface::getJointLimits(urdf_joint, limits))
79  {
80 
81  ThMinPos = limits.min_position;
82  ThMaxPos = limits.max_position;
83 
84  result = true;
85  }
86  }
87  else
88  {
89  ThMinPos=0;
90  ThMaxPos= 0;
91  result = false;
92  }
93 
94  return result;
95  }
96 
98  std::string thumb_opp_passive_joint::getURDF(std::string param_name) const
99  {
100  std::string urdf_string;
101 
102  // search and wait for robot_description on param server
103  while (urdf_string.empty())
104  {
105  std::string search_param_name;
106 
107  if (n.searchParam(param_name, search_param_name))
108  {
109  ROS_INFO_ONCE_NAMED("remap_mia_joint_states", "node is waiting for model"
110  " URDF in parameter on the ROS param server.");
111 
112  n.getParam(search_param_name, urdf_string);
113  }
114  else
115  {
116  ROS_INFO_ONCE_NAMED("remap_mia_joint_states", "Node is waiting for model"
117  " URDF in parameter on the ROS param server.");
118 
119  n.getParam(param_name, urdf_string);
120  }
121 
122  usleep(100000);
123  }
124  ROS_DEBUG_STREAM_NAMED("remap_mia_joint_states", "Recieved urdf from param server, parsing...");
125  return urdf_string;
126  }
127 
128 
129 
130 
131 
132 } // namespace
mia_hand::thumb_opp_passive_joint::~thumb_opp_passive_joint
~thumb_opp_passive_joint()
Class destructor.
Definition: mia_thumb_opp_passivejoints.cpp:35
mia_hand::thumb_opp_passive_joint::getURDF
std::string getURDF(std::string param_name) const
Get the name of the URDF.
Definition: mia_thumb_opp_passivejoints.cpp:98
mia_hand::thumb_opp_passive_joint::GetThumbOppPosition
double GetThumbOppPosition(double j_index_flex_pos)
Evaluate the position of the thumb opposition based on index flexion state.
Definition: mia_thumb_opp_passivejoints.cpp:38
mia_hand::thumb_opp_passive_joint::LoadURDFInfo
bool LoadURDFInfo()
Load Info from loaded URDF.
Definition: mia_thumb_opp_passivejoints.cpp:60
mia_hand::thumb_opp_passive_joint::ThMinPos
double ThMinPos
Lower Limit of the thumb_opp joint.
Definition: mia_thumb_opp_passivejoints.h:104
mia_hand::thumb_opp_passive_joint::j_thumb_name
std::string j_thumb_name
Name of the thumb_opp joint specified in the URDF.
Definition: mia_thumb_opp_passivejoints.h:92
mia_thumb_opp_passivejoints.h
mia_hand::thumb_opp_passive_joint::init
void init(const bool LoadURDFInfo_=false)
Initialize class.
Definition: mia_thumb_opp_passivejoints.cpp:19
mia_hand
namespace containing thumb_opp_passive_joints class.
Definition: mia_thumb_opp_passivejoints.h:24
mia_hand::thumb_opp_passive_joint::thumb_opp_passive_joint
thumb_opp_passive_joint()
Class constructor.
Definition: mia_thumb_opp_passivejoints.cpp:14
mia_hand::thumb_opp_passive_joint::ThMaxPos
double ThMaxPos
Upper Limit of the thumb_opp joint.
Definition: mia_thumb_opp_passivejoints.h:110
mia_hand::thumb_opp_passive_joint::n
ros::NodeHandle n
A ROS node variable.
Definition: mia_thumb_opp_passivejoints.h:117
mia_hand::thumb_opp_passive_joint::robot_description_
std::string robot_description_
Name of parameter describing the robot description loaded in the ROS parameter server.
Definition: mia_thumb_opp_passivejoints.h:98
mia_hand::thumb_opp_passive_joint::j_index_name
std::string j_index_name
Name of the index_fle joint specified in the URDF.
Definition: mia_thumb_opp_passivejoints.h:86