mia_hand_ros_control
rel 1.0.0
|
Go to the documentation of this file.
11 #include <ros/console.h>
13 using transmission_interface::ActuatorData;
14 using transmission_interface::JointData;
32 throw TransmissionInterfaceException(
"Transmission reduction ratio cannot be zero.");
44 double Scale = 7.0074;
46 double mu = Scale * pos + Offset;
53 double Scale = 0.14271;
55 double pos = (double) round( Scale * mu + Offset);
62 double Scale = 65.4167;
64 double omega_m = Scale * spe + Offset;
71 double Scale = 0.015287;
73 double spe = (double) round( Scale * omega_m + Offset);
88 double spe = *act_data.velocity[0];
89 double omega_m =
dh(spe);
92 *jnt_data.velocity[0] = omega_f;
98 double pos = *act_data.position[0];
99 double mu =
h_mrl(pos);
102 *jnt_data.position[0] = fi;
107 ActuatorData& act_data)
110 double omega_f = *jnt_data.velocity[0];
112 double spe =
dh_inv(omega_m);
114 *act_data.velocity[0] = spe;
118 ActuatorData& act_data)
121 double fi = *jnt_data.position[0];
125 *act_data.position[0] = pos;
136 *jnt_data.effort[0] = *act_data.effort[0] * 1;
140 ActuatorData& act_data)
143 *act_data.effort[0] = *jnt_data.effort[0] / 1;
void actuatorToJointVelocity(const transmission_interface::ActuatorData &act_data, transmission_interface::JointData &jnt_data)
Transform velocity variables from actuator to joint space.
double dh(const double spe)
Mrl transmission first step function for velocity: pos = omega_m = dh(spe).
double dh_inv(const double omega_m)
Mrl transmission first step inverse function for velocity: spe = dh_inv(omega_m).
double h_mrl(const double pos)
Mrl transmission first step function for pose: mu = mu = h_mrl(pos).
void actuatorToJointPosition(const transmission_interface::ActuatorData &act_data, transmission_interface::JointData &jnt_data)
Transform position variables from actuator to joint space.
MiaMrlTransmission()
Class constructor.
double h_mrl_inv(const double mu)
Mrl transmission first step inverse function for pose: pos = h_mrl_inv(mu).
void jointToActuatorVelocity(const transmission_interface::JointData &jnt_data, transmission_interface::ActuatorData &act_data)
Transform velocity variables from joint to actuator space.
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.
void jointToActuatorPosition(const transmission_interface::JointData &jnt_data, transmission_interface::ActuatorData &act_data)
Transform position variables from joint to actuator space.
double reduction_
Linear second step reduction of the tranmission.
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.