differential_transmission.h

Go to the documentation of this file.

// Copyright (C) 2013, PAL Robotics S.L.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//   * Redistributions of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//   * Redistributions in binary form must reproduce the above copyright
//     notice, this list of conditions and the following disclaimer in the
//     documentation and/or other materials provided with the distribution.
//   * Neither the name of PAL Robotics S.L. nor the names of its
//     contributors may be used to endorse or promote products derived from
//     this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.


#ifndef TRANSMISSION_INTERFACE_DIFFERENTIAL_TRANSMISSION_H
#define TRANSMISSION_INTERFACE_DIFFERENTIAL_TRANSMISSION_H

#include <cassert>
#include <string>
#include <vector>

#include <transmission_interface/transmission.h>
#include <transmission_interface/transmission_interface_exception.h>

namespace transmission_interface
{

class DifferentialTransmission : public Transmission
{
public:
  DifferentialTransmission(const std::vector<double>& actuator_reduction,
                           const std::vector<double>& joint_reduction,
                           const std::vector<double>& joint_offset = std::vector<double>(2, 0.0));

  void actuatorToJointEffort(const ActuatorData& act_data,
                                   JointData&    jnt_data);

  void actuatorToJointVelocity(const ActuatorData& act_data,
                                     JointData&    jnt_data);

  void actuatorToJointPosition(const ActuatorData& act_data,
                                     JointData&    jnt_data);

  void jointToActuatorEffort(const JointData&    jnt_data,
                                   ActuatorData& act_data);

  void jointToActuatorVelocity(const JointData&    jnt_data,
                                     ActuatorData& act_data);

  void jointToActuatorPosition(const JointData&    jnt_data,
                                     ActuatorData& act_data);

  std::size_t numActuators() const {return 2;}
  std::size_t numJoints()    const {return 2;}

  const std::vector<double>& getActuatorReduction() const {return act_reduction_;}
  const std::vector<double>& getJointReduction()    const {return jnt_reduction_;}
  const std::vector<double>& getJointOffset()       const {return jnt_offset_;}

protected:
  std::vector<double>  act_reduction_;
  std::vector<double>  jnt_reduction_;
  std::vector<double>  jnt_offset_;
};

inline DifferentialTransmission::DifferentialTransmission(const std::vector<double>& actuator_reduction,
                                                          const std::vector<double>& joint_reduction,
                                                          const std::vector<double>& joint_offset)
  : Transmission(),
    act_reduction_(actuator_reduction),
    jnt_reduction_(joint_reduction),
    jnt_offset_(joint_offset)
{
  if (2 != actuator_reduction.size() ||
      2 != jnt_reduction_.size()   ||
      2 != jnt_offset_.size())
  {
    throw TransmissionInterfaceException("Reduction and offset vectors of a differential transmission must have size 2.");
  }

  if (0.0 == actuator_reduction[0] ||
      0.0 == actuator_reduction[1] ||
      0.0 == jnt_reduction_[0]   ||
      0.0 == jnt_reduction_[1]
  )
  {
    throw TransmissionInterfaceException("Transmission reduction ratios cannot be zero.");
  }
}

inline void DifferentialTransmission::actuatorToJointEffort(const ActuatorData& act_data,
                                                                  JointData&    jnt_data)
{
  assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size());
  assert(act_data.effort[0] && act_data.effort[1] && jnt_data.effort[0] && jnt_data.effort[1]);

  std::vector<double>& ar = act_reduction_;
  std::vector<double>& jr = jnt_reduction_;

  *jnt_data.effort[0] = jr[0] * (*act_data.effort[0] * ar[0] + *act_data.effort[1] * ar[1]);
  *jnt_data.effort[1] = jr[1] * (*act_data.effort[0] * ar[0] - *act_data.effort[1] * ar[1]);
}

inline void DifferentialTransmission::actuatorToJointVelocity(const ActuatorData& act_data,
                                                                    JointData&    jnt_data)
{
  assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size());
  assert(act_data.velocity[0] && act_data.velocity[1] && jnt_data.velocity[0] && jnt_data.velocity[1]);

  std::vector<double>& ar = act_reduction_;
  std::vector<double>& jr = jnt_reduction_;

  *jnt_data.velocity[0] = (*act_data.velocity[0] / ar[0] + *act_data.velocity[1] / ar[1]) / (2.0 * jr[0]);
  *jnt_data.velocity[1] = (*act_data.velocity[0] / ar[0] - *act_data.velocity[1] / ar[1]) / (2.0 * jr[1]);
}

inline void DifferentialTransmission::actuatorToJointPosition(const ActuatorData& act_data,
                                                                    JointData&    jnt_data)
{
  assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size());
  assert(act_data.position[0] && act_data.position[1] && jnt_data.position[0] && jnt_data.position[1]);

  std::vector<double>& ar = act_reduction_;
  std::vector<double>& jr = jnt_reduction_;

  *jnt_data.position[0] = (*act_data.position[0] / ar[0] + *act_data.position[1] / ar[1]) / (2.0 * jr[0]) + jnt_offset_[0];
  *jnt_data.position[1] = (*act_data.position[0] / ar[0] - *act_data.position[1] / ar[1]) / (2.0 * jr[1]) + jnt_offset_[1];
}

inline void DifferentialTransmission::jointToActuatorEffort(const JointData&    jnt_data,
                                                                  ActuatorData& act_data)
{
  assert(numActuators() == act_data.effort.size() && numJoints() == jnt_data.effort.size());
  assert(act_data.effort[0] && act_data.effort[1] && jnt_data.effort[0] && jnt_data.effort[1]);

  std::vector<double>& ar = act_reduction_;
  std::vector<double>& jr = jnt_reduction_;

  *act_data.effort[0] = (*jnt_data.effort[0] / jr[0] + *jnt_data.effort[1] / jr[1]) / (2.0 * ar[0]);
  *act_data.effort[1] = (*jnt_data.effort[0] / jr[0] - *jnt_data.effort[1] / jr[1]) / (2.0 * ar[1]);
}

inline void DifferentialTransmission::jointToActuatorVelocity(const JointData&    jnt_data,
                                                                    ActuatorData& act_data)
{
  assert(numActuators() == act_data.velocity.size() && numJoints() == jnt_data.velocity.size());
  assert(act_data.velocity[0] && act_data.velocity[1] && jnt_data.velocity[0] && jnt_data.velocity[1]);

  std::vector<double>& ar = act_reduction_;
  std::vector<double>& jr = jnt_reduction_;

  *act_data.velocity[0] = (*jnt_data.velocity[0] * jr[0] + *jnt_data.velocity[1] * jr[1]) * ar[0];
  *act_data.velocity[1] = (*jnt_data.velocity[0] * jr[0] - *jnt_data.velocity[1] * jr[1]) * ar[1];
}

inline void DifferentialTransmission::jointToActuatorPosition(const JointData&    jnt_data,
                                                                    ActuatorData& act_data)
{
  assert(numActuators() == act_data.position.size() && numJoints() == jnt_data.position.size());
  assert(act_data.position[0] && act_data.position[1] && jnt_data.position[0] && jnt_data.position[1]);

  std::vector<double>& ar = act_reduction_;
  std::vector<double>& jr = jnt_reduction_;

  double jnt_pos_off[2] = {*jnt_data.position[0] - jnt_offset_[0], *jnt_data.position[1] - jnt_offset_[1]};

  *act_data.position[0] = (jnt_pos_off[0] * jr[0] + jnt_pos_off[1] * jr[1]) * ar[0];
  *act_data.position[1] = (jnt_pos_off[0] * jr[0] - jnt_pos_off[1] * jr[1]) * ar[1];
}

} // transmission_interface

#endif // TRANSMISSION_INTERFACE_DIFFERENTIAL_TRANSMISSION_H