mia_hand_ros_control  rel 1.0.0
mia_index_transmission.h
Go to the documentation of this file.
1 /*********************************************************************************************************
2  Author: Prensilia srl
3  Desc: Transmission of the MIA index finger
4 
5  version 1.0
6 **********************************************************************************************************/
7 
8 
9 #ifndef MIA_INDEX_TRANSMISSION_H
10 #define MIA_INDEX_TRANSMISSION_H
11 
12 #include <iostream>
13 #include <vector>
14 #include <cassert>
15 #include <string>
16 #include <cmath>
17 #include <ros/ros.h>
18 
19 #include <transmission_interface/transmission.h>
20 #include <transmission_interface/transmission_interface_exception.h>
21 
22 
24 {
25 
29  class MiaIndexTransmission: public Transmission
30  {
31 
32  public:
33 
38 
43  const transmission_interface::ActuatorData& ind_act_state,
44  transmission_interface::JointData& ind_jnt_data) override;
45 
52  const transmission_interface::ActuatorData& ind_act_state,
53  transmission_interface::JointData& ind_jnt_data) override;
54 
61  const transmission_interface::ActuatorData& ind_act_state,
62  transmission_interface::JointData& ind_jnt_data) override;
63 
68  const transmission_interface::JointData& ind_jnt_data,
69  transmission_interface::ActuatorData& ind_act_cmd) override;
70 
77  const transmission_interface::JointData& ind_jnt_data,
78  transmission_interface::ActuatorData& ind_act_cmd) override;
79 
84  const transmission_interface::JointData& ind_jnt_data,
85  transmission_interface::ActuatorData& act_data) override; // unused
86 
94  const transmission_interface::JointData& ind_jnt_data,
95  const transmission_interface::ActuatorData& ind_act_state,
96  transmission_interface::ActuatorData& ind_act_cmd) ;
97 
103  double h_i(const double pos); // mu = h_i(pos)
104 
110  double h_i_inv(const double mu); // pos = h_i_inv(mu)
111 
117  double dh_i(const double spe); // omega_m = dh_i(spe)
118 
124  double dh_i_inv(const double omega_m); // spe = dh_i_inv(omega_m)
125 
131  double f(const double alpha ); // delta = f(alpha)
132 
138  double f_inv(const double delta ); // alpha = f_inv(delta)
139 
145  double df(const double omega_a ); // // Non linear TR-1
146 
147  // General trasmissions attributes
148  std::size_t numActuators() const {return 1;}
149  std::size_t numJoints() const {return 1;}
150 
151  private:
152  double linear_reduction_;
153 
158  std::vector<double> x_intervals_f;
159 
164  std::vector<double> x_intervals_f_inv;
165 
170  std::vector<double> x_intervals_df;
171 
176  std::vector<double> f_scale;
177 
182  std::vector<double> f_offset;
183 
188  std::vector<double> f_inv_scale;
189 
194  std::vector<double> f_inv_offset;
195 
200  std::vector<double> df_scale;
201 
206  std::vector<double> df_offset;
207 
208 
209  };
210 
211 
212 
213 } // namespace
214 
215 #endif //MIA_INDEX_TRANSMISSION_H
transmission_interface::MiaIndexTransmission::df_offset
std::vector< double > df_offset
Offset factors of the df funtion.
Definition: mia_index_transmission.h:216
transmission_interface::MiaIndexTransmission::f
double f(const double alpha)
Index transmission second step function for position: delta = f(alpha)
Definition: mia_index_transmission.cpp:100
transmission_interface
Definition: mia_index_transmission.h:23
transmission_interface::MiaIndexTransmission::jointToActuatorEffort
void jointToActuatorEffort(const transmission_interface::JointData &ind_jnt_data, transmission_interface::ActuatorData &ind_act_cmd) override
Do not use this methos since Mia hand has not effort control.
Definition: mia_index_transmission.cpp:346
transmission_interface::MiaIndexTransmission::f_inv_offset
std::vector< double > f_inv_offset
Offset factors of the f_inv funtion.
Definition: mia_index_transmission.h:204
transmission_interface::MiaIndexTransmission::df
double df(const double omega_a)
Index transmission second step function for velocity: alpha = f_inv(delta)
Definition: mia_index_transmission.cpp:185
transmission_interface::MiaIndexTransmission::x_intervals_f_inv
std::vector< double > x_intervals_f_inv
Upper limiits of the inputs intervals of the f_inv funtion.
Definition: mia_index_transmission.h:174
transmission_interface::MiaIndexTransmission::f_offset
std::vector< double > f_offset
Offset factors of the f funtion.
Definition: mia_index_transmission.h:192
transmission_interface::MiaIndexTransmission::actuatorToJointVelocity
void actuatorToJointVelocity(const transmission_interface::ActuatorData &ind_act_state, transmission_interface::JointData &ind_jnt_data) override
Transform velocity variables from actuator to joint space.
Definition: mia_index_transmission.cpp:256
transmission_interface::MiaIndexTransmission
Implementation of the no-linear Mia Index Transmission.
Definition: mia_index_transmission.h:34
transmission_interface::MiaIndexTransmission::df_scale
std::vector< double > df_scale
Scale factors of the df funtion.
Definition: mia_index_transmission.h:210
transmission_interface::MiaIndexTransmission::linear_reduction_
double linear_reduction_
Definition: mia_index_transmission.h:162
transmission_interface::MiaIndexTransmission::jointToActuatorVelocity
void jointToActuatorVelocity(const transmission_interface::JointData &ind_jnt_data, transmission_interface::ActuatorData &act_data) override
unused
Definition: mia_index_transmission.cpp:321
transmission_interface::MiaIndexTransmission::actuatorToJointEffort
void actuatorToJointEffort(const transmission_interface::ActuatorData &ind_act_state, transmission_interface::JointData &ind_jnt_data) override
Do not use this methos since Mia hand has not effort control.
Definition: mia_index_transmission.cpp:334
transmission_interface::MiaIndexTransmission::f_scale
std::vector< double > f_scale
Scales factors of the f funtion.
Definition: mia_index_transmission.h:186
transmission_interface::MiaIndexTransmission::IndexjointToActuatorVelocity
void IndexjointToActuatorVelocity(const transmission_interface::JointData &ind_jnt_data, const transmission_interface::ActuatorData &ind_act_state, transmission_interface::ActuatorData &ind_act_cmd)
Transform velocity variables from joint to actuator space.
Definition: mia_index_transmission.cpp:297
transmission_interface::MiaIndexTransmission::dh_i_inv
double dh_i_inv(const double omega_m)
Index transmission first step inverse function for velocity: spe = dh_i_inv(omega_m)
Definition: mia_index_transmission.cpp:90
transmission_interface::MiaIndexTransmission::actuatorToJointPosition
void actuatorToJointPosition(const transmission_interface::ActuatorData &ind_act_state, transmission_interface::JointData &ind_jnt_data) override
Transform position variables from actuator to joint space.
Definition: mia_index_transmission.cpp:240
transmission_interface::MiaIndexTransmission::x_intervals_df
std::vector< double > x_intervals_df
Upper limiits of the inputs intervals of the df funtion.
Definition: mia_index_transmission.h:180
transmission_interface::MiaIndexTransmission::dh_i
double dh_i(const double spe)
Index transmission first step function for velocity: omega_m = dh_i(spe).
Definition: mia_index_transmission.cpp:81
transmission_interface::MiaIndexTransmission::f_inv
double f_inv(const double delta)
Index transmission second step inverse function for position: alpha = f_inv(delta)
Definition: mia_index_transmission.cpp:143
transmission_interface::MiaIndexTransmission::h_i
double h_i(const double pos)
Index transmission first step function for pose: mu = h_i(pos).
Definition: mia_index_transmission.cpp:63
transmission_interface::MiaIndexTransmission::x_intervals_f
std::vector< double > x_intervals_f
Upper limiits of the inputs intervals of the f funtion.
Definition: mia_index_transmission.h:168
transmission_interface::MiaIndexTransmission::f_inv_scale
std::vector< double > f_inv_scale
Scale factors of the f_inv funtion.
Definition: mia_index_transmission.h:198
transmission_interface::MiaIndexTransmission::h_i_inv
double h_i_inv(const double mu)
Index transmission first step inverse function for pose: pos = h_i_inv(mu)
Definition: mia_index_transmission.cpp:72
transmission_interface::MiaIndexTransmission::jointToActuatorPosition
void jointToActuatorPosition(const transmission_interface::JointData &ind_jnt_data, transmission_interface::ActuatorData &ind_act_cmd) override
Transform position variables from joint to actuator space.
Definition: mia_index_transmission.cpp:280
transmission_interface::MiaIndexTransmission::MiaIndexTransmission
MiaIndexTransmission()
Class constructor.
Definition: mia_index_transmission.cpp:21