differential_transmission.h
Go to the documentation of this file.
00001 
00002 // Copyright (C) 2013, PAL Robotics S.L.
00003 //
00004 // Redistribution and use in source and binary forms, with or without
00005 // modification, are permitted provided that the following conditions are met:
00006 //   * Redistributions of source code must retain the above copyright notice,
00007 //     this list of conditions and the following disclaimer.
00008 //   * Redistributions in binary form must reproduce the above copyright
00009 //     notice, this list of conditions and the following disclaimer in the
00010 //     documentation and/or other materials provided with the distribution.
00011 //   * Neither the name of PAL Robotics S.L. nor the names of its
00012 //     contributors may be used to endorse or promote products derived from
00013 //     this software without specific prior written permission.
00014 //
00015 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025 // POSSIBILITY OF SUCH DAMAGE.
00027 
00029 
00030 #ifndef TRANSMISSION_INTERFACE_DIFFERENTIAL_TRANSMISSION_H
00031 #define TRANSMISSION_INTERFACE_DIFFERENTIAL_TRANSMISSION_H
00032 
00033 #include <cassert>
00034 #include <string>
00035 #include <vector>
00036 
00037 #include <transmission_interface/transmission.h>
00038 #include <transmission_interface/transmission_interface_exception.h>
00039 
00040 namespace transmission_interface
00041 {
00042 
00117 class DifferentialTransmission : public Transmission
00118 {
00119 public:
00126   DifferentialTransmission(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 DifferentialTransmission::DifferentialTransmission(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 (2 != actuator_reduction.size() ||
00212       2 != jnt_reduction_.size()   ||
00213       2 != jnt_offset_.size())
00214   {
00215     throw TransmissionInterfaceException("Reduction and offset vectors of a differential transmission must have size 2.");
00216   }
00217 
00218   if (0.0 == actuator_reduction[0] ||
00219       0.0 == actuator_reduction[1] ||
00220       0.0 == jnt_reduction_[0]   ||
00221       0.0 == jnt_reduction_[1]
00222   )
00223   {
00224     throw TransmissionInterfaceException("Transmission reduction ratios cannot be zero.");
00225   }
00226 }
00227 
00228 inline void DifferentialTransmission::actuatorToJointEffort(const ActuatorData& act_data,
00229                                                                   JointData&    jnt_data)
00230 {
00231   assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size());
00232   assert(act_data.effort[0] && act_data.effort[1] && jnt_data.effort[0] && jnt_data.effort[1]);
00233 
00234   std::vector<double>& ar = act_reduction_;
00235   std::vector<double>& jr = jnt_reduction_;
00236 
00237   *jnt_data.effort[0] = jr[0] * (*act_data.effort[0] * ar[0] + *act_data.effort[1] * ar[1]);
00238   *jnt_data.effort[1] = jr[1] * (*act_data.effort[0] * ar[0] - *act_data.effort[1] * ar[1]);
00239 }
00240 
00241 inline void DifferentialTransmission::actuatorToJointVelocity(const ActuatorData& act_data,
00242                                                                     JointData&    jnt_data)
00243 {
00244   assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size());
00245   assert(act_data.velocity[0] && act_data.velocity[1] && jnt_data.velocity[0] && jnt_data.velocity[1]);
00246 
00247   std::vector<double>& ar = act_reduction_;
00248   std::vector<double>& jr = jnt_reduction_;
00249 
00250   *jnt_data.velocity[0] = (*act_data.velocity[0] / ar[0] + *act_data.velocity[1] / ar[1]) / (2.0 * jr[0]);
00251   *jnt_data.velocity[1] = (*act_data.velocity[0] / ar[0] - *act_data.velocity[1] / ar[1]) / (2.0 * jr[1]);
00252 }
00253 
00254 inline void DifferentialTransmission::actuatorToJointPosition(const ActuatorData& act_data,
00255                                                                     JointData&    jnt_data)
00256 {
00257   assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size());
00258   assert(act_data.position[0] && act_data.position[1] && jnt_data.position[0] && jnt_data.position[1]);
00259 
00260   std::vector<double>& ar = act_reduction_;
00261   std::vector<double>& jr = jnt_reduction_;
00262 
00263   *jnt_data.position[0] = (*act_data.position[0] / ar[0] + *act_data.position[1] / ar[1]) / (2.0 * jr[0]) + jnt_offset_[0];
00264   *jnt_data.position[1] = (*act_data.position[0] / ar[0] - *act_data.position[1] / ar[1]) / (2.0 * jr[1]) + jnt_offset_[1];
00265 }
00266 
00267 inline void DifferentialTransmission::jointToActuatorEffort(const JointData&    jnt_data,
00268                                                                   ActuatorData& act_data)
00269 {
00270   assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size());
00271   assert(act_data.effort[0] && act_data.effort[1] && jnt_data.effort[0] && jnt_data.effort[1]);
00272 
00273   std::vector<double>& ar = act_reduction_;
00274   std::vector<double>& jr = jnt_reduction_;
00275 
00276   *act_data.effort[0] = (*jnt_data.effort[0] / jr[0] + *jnt_data.effort[1] / jr[1]) / (2.0 * ar[0]);
00277   *act_data.effort[1] = (*jnt_data.effort[0] / jr[0] - *jnt_data.effort[1] / jr[1]) / (2.0 * ar[1]);
00278 }
00279 
00280 inline void DifferentialTransmission::jointToActuatorVelocity(const JointData&    jnt_data,
00281                                                                     ActuatorData& act_data)
00282 {
00283   assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size());
00284   assert(act_data.velocity[0] && act_data.velocity[1] && jnt_data.velocity[0] && jnt_data.velocity[1]);
00285 
00286   std::vector<double>& ar = act_reduction_;
00287   std::vector<double>& jr = jnt_reduction_;
00288 
00289   *act_data.velocity[0] = (*jnt_data.velocity[0] * jr[0] + *jnt_data.velocity[1] * jr[1]) * ar[0];
00290   *act_data.velocity[1] = (*jnt_data.velocity[0] * jr[0] - *jnt_data.velocity[1] * jr[1]) * ar[1];
00291 }
00292 
00293 inline void DifferentialTransmission::jointToActuatorPosition(const JointData&    jnt_data,
00294                                                                     ActuatorData& act_data)
00295 {
00296   assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size());
00297   assert(act_data.position[0] && act_data.position[1] && jnt_data.position[0] && jnt_data.position[1]);
00298 
00299   std::vector<double>& ar = act_reduction_;
00300   std::vector<double>& jr = jnt_reduction_;
00301 
00302   double jnt_pos_off[2] = {*jnt_data.position[0] - jnt_offset_[0], *jnt_data.position[1] - jnt_offset_[1]};
00303 
00304   *act_data.position[0] = (jnt_pos_off[0] * jr[0] + jnt_pos_off[1] * jr[1]) * ar[0];
00305   *act_data.position[1] = (jnt_pos_off[0] * jr[0] - jnt_pos_off[1] * jr[1]) * ar[1];
00306 }
00307 
00308 } // transmission_interface
00309 
00310 #endif // TRANSMISSION_INTERFACE_DIFFERENTIAL_TRANSMISSION_H


transmission_interface
Author(s): Adolfo Rodriguez Tsouroukdissian
autogenerated on Fri Aug 28 2015 12:36:26