simple_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.


#pragma once


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

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

namespace transmission_interface
{

class SimpleTransmission : public Transmission
{
public:
  SimpleTransmission(const double reduction,
                     const double joint_offset = 0.0);

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

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

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

  void actuatorToJointAbsolutePosition(const ActuatorData& act_data,
                                             JointData&    jnt_data) override;

  void actuatorToJointTorqueSensor(const ActuatorData& act_data,
                                         JointData&    jnt_data) override;

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

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

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

  std::size_t numActuators() const override {return 1;}
  std::size_t numJoints()    const override {return 1;}
  bool hasActuatorToJointAbsolutePosition() const override {return true;}
  bool hasActuatorToJointTorqueSensor()     const override {return true;}

  double getActuatorReduction() const {return reduction_;}
  double getJointOffset()       const {return jnt_offset_;}


private:
  double reduction_;
  double jnt_offset_;
};

inline SimpleTransmission::SimpleTransmission(const double reduction,
                                              const double joint_offset)
  : reduction_(reduction),
    jnt_offset_(joint_offset)
{
  if (0.0 == reduction_)
  {
    throw TransmissionInterfaceException("Transmission reduction ratio cannot be zero.");
  }
}

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

  *jnt_data.effort[0] = *act_data.effort[0] * reduction_;
}

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

  *jnt_data.velocity[0] = *act_data.velocity[0] / reduction_;
}

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

  *jnt_data.position[0] = *act_data.position[0] / reduction_ + jnt_offset_;
}

inline void SimpleTransmission::actuatorToJointAbsolutePosition(const ActuatorData& act_data,
                                                                      JointData&    jnt_data)
{

  assert(numActuators() == act_data.absolute_position.size() && numJoints() == jnt_data.absolute_position.size());
  assert(act_data.absolute_position[0] && jnt_data.absolute_position[0]);

  *jnt_data.absolute_position[0] = *act_data.absolute_position[0] / reduction_ + jnt_offset_;
}

inline void SimpleTransmission::actuatorToJointTorqueSensor(const ActuatorData& act_data,
                                                                  JointData&    jnt_data)
{

  assert(numActuators() == act_data.torque_sensor.size() && numJoints() == jnt_data.torque_sensor.size());
  assert(act_data.torque_sensor[0] && jnt_data.torque_sensor[0]);

  *jnt_data.torque_sensor[0] = *act_data.torque_sensor[0] * reduction_;
}


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

  *act_data.effort[0] = *jnt_data.effort[0] / reduction_;
}

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

  *act_data.velocity[0] = *jnt_data.velocity[0] * reduction_;
}

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

  *act_data.position[0] = (*jnt_data.position[0] - jnt_offset_) * reduction_;
}

} // transmission_interface