mia_hand_ros_control  rel 1.0.0
mia_thfle_transmission.cpp
Go to the documentation of this file.
1  /*********************************************************************************************************
2  Modifyed by Author: Prensilia srl
3  Desc: Transmission interfae of the MIA thumbe flexion finger
4 
5  version 1.0
6 
7 **********************************************************************************************************/
8 
10 #include <math.h>
11 
12 using transmission_interface::ActuatorData;
13 using transmission_interface::JointData;
14 
15 namespace transmission_interface
16 {
17 
19 //MiaThfleTransmission : constructor
21 
23  : Transmission()
24 {
25 
26  reduction_ = 64.0 * 40.0;
27 
28 
29  if (0.0 == reduction_)
30  {
31  throw TransmissionInterfaceException("Transmission reduction ratio cannot be zero.");
32  }
33 }
34 
35 
37 // Simple functions needed to evaluate trasmission ratio
39 
40 // mu = h_thfle(pos)
41 inline double MiaThfleTransmission::h_thfle(const double pos)
42 {
43  double Scale = 11.3895;
44  double Offset = 0.0;
45  double mu = Scale * pos + Offset;
46  return mu;
47 }
48 
49 // pos = h_thfle_inv(mu)
50 double MiaThfleTransmission::h_thfle_inv(const double mu)
51 {
52  double Scale = 0.0878;
53  double Offset = 0;
54  double pos = (double) round( Scale * mu + Offset); // cmd
55  return pos;
56 }
57 
58 // omega_m = dh(spe)
59 double MiaThfleTransmission::dh (const double spe)
60 {
61  double Scale = 65.4167;
62  double Offset = 0;
63  double omega_m = Scale * spe + Offset;
64  return omega_m;
65 }
66 
67 // spe = dh_i_inv(omega_m)
68 double MiaThfleTransmission::dh_inv(const double omega_m)
69 {
70  double Scale = 0.015287;
71  double Offset = 0;
72  double spe = (double) round( Scale * omega_m + Offset); // cmd
73  return spe;
74 }
75 
76 
77 
78 
80 // Macro functions of for the ros trasmission
82 
83 void MiaThfleTransmission::actuatorToJointVelocity(const ActuatorData& act_data,
84  JointData& jnt_data)
85 {
86 
87  double spe = *act_data.velocity[0];
88  double omega_m = dh(spe);
89  double omega_f = omega_m / reduction_;
90 
91  *jnt_data.velocity[0] = omega_f;
92 }
93 
94 void MiaThfleTransmission::actuatorToJointPosition(const ActuatorData& act_data,
95  JointData& jnt_data)
96 {
97  double pos = *act_data.position[0];
98  double mu = h_thfle(pos);
99  double fi = mu / reduction_;
100 
101  *jnt_data.position[0] = fi;
102 }
103 
104 
105 void MiaThfleTransmission::jointToActuatorVelocity(const JointData& jnt_data,
106  ActuatorData& act_data)
107 {
108 
109  double omega_f = *jnt_data.velocity[0];
110  double omega_m = omega_f * reduction_;
111  double spe = dh_inv(omega_m);
112 
113  *act_data.velocity[0] = spe;
114 }
115 
116 void MiaThfleTransmission::jointToActuatorPosition(const JointData& jnt_data,
117  ActuatorData& act_data)
118 {
119 
120  double fi = *jnt_data.position[0];
121  double mu = fi * reduction_;
122  double pos = h_thfle_inv(mu);
123 
124  *act_data.position[0] = pos;
125 }
126 
127 
129 // NOT TO BE USED
130 
131 void MiaThfleTransmission::actuatorToJointEffort(const ActuatorData& act_data,
132  JointData& jnt_data)
133 {
134 
135  *jnt_data.effort[0] = *act_data.effort[0] * 1;
136 }
137 
138 void MiaThfleTransmission::jointToActuatorEffort(const JointData& jnt_data,
139  ActuatorData& act_data)
140 {
141 
142  *act_data.effort[0] = *jnt_data.effort[0] / 1;
143 }
144 
146 
147 }// namespace mia_transmission_interface
transmission_interface::MiaThfleTransmission::jointToActuatorVelocity
void jointToActuatorVelocity(const transmission_interface::JointData &jnt_data, transmission_interface::ActuatorData &act_data)
Transform velocity variables from joint to actuator space.
Definition: mia_thfle_transmission.cpp:105
transmission_interface
Definition: mia_index_transmission.h:23
transmission_interface::MiaThfleTransmission::dh_inv
double dh_inv(const double omega_m)
Thumb flexion transmission first step inverse function for velocity: spe = dh_inv(omega_m).
Definition: mia_thfle_transmission.cpp:68
mia_thfle_transmission.h
transmission_interface::MiaThfleTransmission::actuatorToJointVelocity
void actuatorToJointVelocity(const transmission_interface::ActuatorData &act_data, transmission_interface::JointData &jnt_data)
Transform velocity variables from actuator to joint space.
Definition: mia_thfle_transmission.cpp:83
transmission_interface::MiaThfleTransmission::h_thfle_inv
double h_thfle_inv(const double mu)
Thumb flexion transmission first step inverse function for pose: pos = h_thfle_inv(mu).
Definition: mia_thfle_transmission.cpp:50
transmission_interface::MiaThfleTransmission::actuatorToJointPosition
void actuatorToJointPosition(const transmission_interface::ActuatorData &act_data, transmission_interface::JointData &jnt_data)
Transform position variables from actuator to joint space.
Definition: mia_thfle_transmission.cpp:94
transmission_interface::MiaThfleTransmission::h_thfle
double h_thfle(const double pos)
Thumb flexion transmission first step function for pose: mu = mu = h_thfle(pos).
Definition: mia_thfle_transmission.cpp:41
transmission_interface::MiaThfleTransmission::actuatorToJointEffort
void actuatorToJointEffort(const transmission_interface::ActuatorData &act_data, transmission_interface::JointData &jnt_data)
Do not use this methos since Mia hand has not effort control.
Definition: mia_thfle_transmission.cpp:131
transmission_interface::MiaThfleTransmission::jointToActuatorPosition
void jointToActuatorPosition(const transmission_interface::JointData &jnt_data, transmission_interface::ActuatorData &act_data)
Transform position variables from joint to actuator space.
Definition: mia_thfle_transmission.cpp:116
transmission_interface::MiaThfleTransmission::reduction_
double reduction_
Linear second step reduction of the tranmission.
Definition: mia_thfle_transmission.h:160
transmission_interface::MiaThfleTransmission::dh
double dh(const double spe)
Thumb flexion transmission first step function for velocity: omega_m = dh(spe)
Definition: mia_thfle_transmission.cpp:59
transmission_interface::MiaThfleTransmission::MiaThfleTransmission
MiaThfleTransmission()
Class constructor.
Definition: mia_thfle_transmission.cpp:22
transmission_interface::MiaThfleTransmission::jointToActuatorEffort
void jointToActuatorEffort(const transmission_interface::JointData &jnt_data, transmission_interface::ActuatorData &act_data)
Do not use this methos since Mia hand has not effort control.
Definition: mia_thfle_transmission.cpp:138