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