Go to the documentation of this file.00001
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00028
00030
00031 #ifndef TRANSMISSION_INTERFACE_FOUR_BAR_LINKAGE_TRANSMISSION_H
00032 #define TRANSMISSION_INTERFACE_FOUR_BAR_LINKAGE_TRANSMISSION_H
00033
00034 #include <cassert>
00035 #include <string>
00036 #include <vector>
00037
00038 #include <transmission_interface/transmission.h>
00039 #include <transmission_interface/transmission_interface_exception.h>
00040
00041 namespace transmission_interface
00042 {
00043
00117 class FourBarLinkageTransmission : public Transmission
00118 {
00119 public:
00126 FourBarLinkageTransmission(const std::vector<double>& actuator_reduction,
00127 const std::vector<double>& joint_reduction,
00128 const std::vector<double>& joint_offset = std::vector<double>(2, 0.0));
00129
00137 void actuatorToJointEffort(const ActuatorData& act_data,
00138 JointData& jnt_data);
00139
00147 void actuatorToJointVelocity(const ActuatorData& act_data,
00148 JointData& jnt_data);
00149
00157 void actuatorToJointPosition(const ActuatorData& act_data,
00158 JointData& jnt_data);
00159
00167 void jointToActuatorEffort(const JointData& jnt_data,
00168 ActuatorData& act_data);
00169
00177 void jointToActuatorVelocity(const JointData& jnt_data,
00178 ActuatorData& act_data);
00179
00187 void jointToActuatorPosition(const JointData& jnt_data,
00188 ActuatorData& act_data);
00189
00190 std::size_t numActuators() const {return 2;}
00191 std::size_t numJoints() const {return 2;}
00192
00193 const std::vector<double>& getActuatorReduction() const {return act_reduction_;}
00194 const std::vector<double>& getJointReduction() const {return jnt_reduction_;}
00195 const std::vector<double>& getJointOffset() const {return jnt_offset_;}
00196
00197 protected:
00198 std::vector<double> act_reduction_;
00199 std::vector<double> jnt_reduction_;
00200 std::vector<double> jnt_offset_;
00201 };
00202
00203 inline FourBarLinkageTransmission::FourBarLinkageTransmission(const std::vector<double>& actuator_reduction,
00204 const std::vector<double>& joint_reduction,
00205 const std::vector<double>& joint_offset)
00206 : Transmission(),
00207 act_reduction_(actuator_reduction),
00208 jnt_reduction_(joint_reduction),
00209 jnt_offset_(joint_offset)
00210 {
00211 if (numActuators() != act_reduction_.size() ||
00212 numJoints() != jnt_reduction_.size() ||
00213 numJoints() != jnt_offset_.size())
00214 {
00215 throw TransmissionInterfaceException("Reduction and offset vectors of a four-bar linkage transmission must have size 2.");
00216 }
00217 if (0.0 == act_reduction_[0] ||
00218 0.0 == act_reduction_[1] ||
00219 0.0 == jnt_reduction_[0] ||
00220 0.0 == jnt_reduction_[1])
00221 {
00222 throw TransmissionInterfaceException("Transmission reduction ratios cannot be zero.");
00223 }
00224 }
00225
00226 inline void FourBarLinkageTransmission::actuatorToJointEffort(const ActuatorData& act_data,
00227 JointData& jnt_data)
00228 {
00229 assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size());
00230 assert(act_data.effort[0] && act_data.effort[1] && jnt_data.effort[0] && jnt_data.effort[1]);
00231
00232 std::vector<double>& ar = act_reduction_;
00233 std::vector<double>& jr = jnt_reduction_;
00234
00235 *jnt_data.effort[0] = jr[0] * (*act_data.effort[0] * ar[0]);
00236 *jnt_data.effort[1] = jr[1] * (*act_data.effort[1] * ar[1] - *act_data.effort[0] * ar[0] * jr[0]);
00237 }
00238
00239 inline void FourBarLinkageTransmission::actuatorToJointVelocity(const ActuatorData& act_data,
00240 JointData& jnt_data)
00241 {
00242 assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size());
00243 assert(act_data.velocity[0] && act_data.velocity[1] && jnt_data.velocity[0] && jnt_data.velocity[1]);
00244
00245 std::vector<double>& ar = act_reduction_;
00246 std::vector<double>& jr = jnt_reduction_;
00247
00248 *jnt_data.velocity[0] = *act_data.velocity[0] / (jr[0] * ar[0]);
00249 *jnt_data.velocity[1] = (*act_data.velocity[1] / ar[1] - *act_data.velocity[0] / (jr[0] * ar[0])) / jr[1];
00250 }
00251
00252 inline void FourBarLinkageTransmission::actuatorToJointPosition(const ActuatorData& act_data,
00253 JointData& jnt_data)
00254 {
00255 assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size());
00256 assert(act_data.position[0] && act_data.position[1] && jnt_data.position[0] && jnt_data.position[1]);
00257
00258 std::vector<double>& ar = act_reduction_;
00259 std::vector<double>& jr = jnt_reduction_;
00260
00261 *jnt_data.position[0] = *act_data.position[0] /(jr[0] * ar[0]) + jnt_offset_[0];
00262 *jnt_data.position[1] = (*act_data.position[1] / ar[1] - *act_data.position[0] / (jr[0] * ar[0])) / jr[1]
00263 + jnt_offset_[1];
00264 }
00265
00266 inline void FourBarLinkageTransmission::jointToActuatorEffort(const JointData& jnt_data,
00267 ActuatorData& act_data)
00268 {
00269 assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size());
00270 assert(act_data.effort[0] && act_data.effort[1] && jnt_data.effort[0] && jnt_data.effort[1]);
00271
00272 std::vector<double>& ar = act_reduction_;
00273 std::vector<double>& jr = jnt_reduction_;
00274
00275 *act_data.effort[0] = *jnt_data.effort[0] / (ar[0] * jr[0]);
00276 *act_data.effort[1] = (*jnt_data.effort[0] + *jnt_data.effort[1] / jr[1]) / ar[1];
00277 }
00278
00279 inline void FourBarLinkageTransmission::jointToActuatorVelocity(const JointData& jnt_data,
00280 ActuatorData& act_data)
00281 {
00282 assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size());
00283 assert(act_data.velocity[0] && act_data.velocity[1] && jnt_data.velocity[0] && jnt_data.velocity[1]);
00284
00285 std::vector<double>& ar = act_reduction_;
00286 std::vector<double>& jr = jnt_reduction_;
00287
00288 *act_data.velocity[0] = *jnt_data.velocity[0] * jr[0] * ar[0];
00289 *act_data.velocity[1] = (*jnt_data.velocity[0] + *jnt_data.velocity[1] * jr[1]) * ar[1];
00290 }
00291
00292 inline void FourBarLinkageTransmission::jointToActuatorPosition(const JointData& jnt_data,
00293 ActuatorData& act_data)
00294 {
00295 assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size());
00296 assert(act_data.position[0] && act_data.position[1] && jnt_data.position[0] && jnt_data.position[1]);
00297
00298 std::vector<double>& ar = act_reduction_;
00299 std::vector<double>& jr = jnt_reduction_;
00300
00301 double jnt_pos_off[2] = {*jnt_data.position[0] - jnt_offset_[0], *jnt_data.position[1] - jnt_offset_[1]};
00302
00303 *act_data.position[0] = jnt_pos_off[0] * jr[0] * ar[0];
00304 *act_data.position[1] = (jnt_pos_off[0] + jnt_pos_off[1] * jr[1]) * ar[1];
00305 }
00306
00307 }
00308
00309 #endif // TRANSMISSION_INTERFACE_FOUR_BAR_LINKAGE_TRANSMISSION_H