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:
00125 FourBarLinkageTransmission(const std::vector<double>& actuator_reduction,
00126 const std::vector<double>& joint_offset = std::vector<double>(2, 0.0));
00127
00135 void actuatorToJointEffort(const ActuatorData& act_data,
00136 JointData& jnt_data);
00137
00145 void actuatorToJointVelocity(const ActuatorData& act_data,
00146 JointData& jnt_data);
00147
00155 void actuatorToJointPosition(const ActuatorData& act_data,
00156 JointData& jnt_data);
00157
00165 void jointToActuatorEffort(const JointData& jnt_data,
00166 ActuatorData& act_data);
00167
00175 void jointToActuatorVelocity(const JointData& jnt_data,
00176 ActuatorData& act_data);
00177
00185 void jointToActuatorPosition(const JointData& jnt_data,
00186 ActuatorData& act_data);
00187
00188 std::size_t numActuators() const {return 2;}
00189 std::size_t numJoints() const {return 2;}
00190
00191 const std::vector<double>& getActuatorReduction() const {return act_reduction_;}
00192 const std::vector<double>& getJointOffset() const {return jnt_offset_;}
00193
00194 protected:
00195 std::vector<double> act_reduction_;
00196 std::vector<double> jnt_offset_;
00197 };
00198
00199 inline FourBarLinkageTransmission::FourBarLinkageTransmission(const std::vector<double>& actuator_reduction,
00200 const std::vector<double>& joint_offset)
00201 : Transmission(),
00202 act_reduction_(actuator_reduction),
00203 jnt_offset_(joint_offset)
00204 {
00205 if (2 != actuator_reduction.size() ||
00206 2 != jnt_offset_.size())
00207 {
00208 throw TransmissionInterfaceException("Reduction and offset vectors of a four-bar linkage transmission must have size 2.");
00209 }
00210 if (0.0 == actuator_reduction[0] ||
00211 0.0 == actuator_reduction[1])
00212 {
00213 throw TransmissionInterfaceException("Transmission reduction ratios cannot be zero.");
00214 }
00215 }
00216
00217 inline void FourBarLinkageTransmission::actuatorToJointEffort(const ActuatorData& act_data,
00218 JointData& jnt_data)
00219 {
00220 assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size());
00221 assert(act_data.effort[0] && act_data.effort[1] && jnt_data.effort[0] && jnt_data.effort[1]);
00222
00223 std::vector<double>& ar = act_reduction_;
00224
00225 *jnt_data.effort[0] = *act_data.effort[0] * ar[0];
00226 *jnt_data.effort[1] = *act_data.effort[1] * ar[1] - *act_data.effort[0] * ar[0];
00227 }
00228
00229 inline void FourBarLinkageTransmission::actuatorToJointVelocity(const ActuatorData& act_data,
00230 JointData& jnt_data)
00231 {
00232 assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size());
00233 assert(act_data.velocity[0] && act_data.velocity[1] && jnt_data.velocity[0] && jnt_data.velocity[1]);
00234
00235 std::vector<double>& ar = act_reduction_;
00236
00237 *jnt_data.velocity[0] = *act_data.velocity[0] / ar[0];
00238 *jnt_data.velocity[1] = *act_data.velocity[1] / ar[1] - *act_data.velocity[0] / ar[0];
00239 }
00240
00241 inline void FourBarLinkageTransmission::actuatorToJointPosition(const ActuatorData& act_data,
00242 JointData& jnt_data)
00243 {
00244 assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size());
00245 assert(act_data.position[0] && act_data.position[1] && jnt_data.position[0] && jnt_data.position[1]);
00246
00247 std::vector<double>& ar = act_reduction_;
00248
00249 *jnt_data.position[0] = *act_data.position[0] / ar[0] + jnt_offset_[0];
00250 *jnt_data.position[1] = *act_data.position[1] / ar[1] - *act_data.position[0] / ar[0] + jnt_offset_[1];
00251 }
00252
00253 inline void FourBarLinkageTransmission::jointToActuatorEffort(const JointData& jnt_data,
00254 ActuatorData& act_data)
00255 {
00256 assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size());
00257 assert(act_data.effort[0] && act_data.effort[1] && jnt_data.effort[0] && jnt_data.effort[1]);
00258
00259 std::vector<double>& ar = act_reduction_;
00260
00261 *act_data.effort[0] = *jnt_data.effort[0] / ar[0];
00262 *act_data.effort[1] = (*jnt_data.effort[0] + *jnt_data.effort[1]) / ar[1];
00263 }
00264
00265 inline void FourBarLinkageTransmission::jointToActuatorVelocity(const JointData& jnt_data,
00266 ActuatorData& act_data)
00267 {
00268 assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size());
00269 assert(act_data.velocity[0] && act_data.velocity[1] && jnt_data.velocity[0] && jnt_data.velocity[1]);
00270
00271 std::vector<double>& ar = act_reduction_;
00272
00273 *act_data.velocity[0] = *jnt_data.velocity[0] * ar[0];
00274 *act_data.velocity[1] = (*jnt_data.velocity[0] + *jnt_data.velocity[1]) * ar[1];
00275 }
00276
00277 inline void FourBarLinkageTransmission::jointToActuatorPosition(const JointData& jnt_data,
00278 ActuatorData& act_data)
00279 {
00280 assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size());
00281 assert(act_data.position[0] && act_data.position[1] && jnt_data.position[0] && jnt_data.position[1]);
00282
00283 std::vector<double>& ar = act_reduction_;
00284
00285 double jnt_pos_off[2] = {*jnt_data.position[0] - jnt_offset_[0], *jnt_data.position[1] - jnt_offset_[1]};
00286
00287 *act_data.position[0] = jnt_pos_off[0] * ar[0];
00288 *act_data.position[1] = (jnt_pos_off[0] + jnt_pos_off[1]) * ar[1];
00289 }
00290
00291 }
00292
00293 #endif // TRANSMISSION_INTERFACE_FOUR_BAR_LINKAGE_TRANSMISSION_H