four_bar_linkage_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 FourBarLinkageTransmission : public Transmission
{
public:
  FourBarLinkageTransmission(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) 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 2;}
  std::size_t numJoints()    const override {return 2;}
  bool hasActuatorToJointAbsolutePosition()  const override {return true;}
  bool hasActuatorToJointTorqueSensor()      const override {return true;}

  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 FourBarLinkageTransmission::FourBarLinkageTransmission(const std::vector<double>& actuator_reduction,
                                                              const std::vector<double>& joint_reduction,
                                                              const std::vector<double>& joint_offset)
  : act_reduction_(actuator_reduction),
    jnt_reduction_(joint_reduction),
    jnt_offset_(joint_offset)
{
  if (numActuators() != act_reduction_.size() ||
      numJoints()    != jnt_reduction_.size() ||
      numJoints()    != jnt_offset_.size())
  {
    throw TransmissionInterfaceException("Reduction and offset vectors of a four-bar linkage transmission must have size 2.");
  }
  if (0.0 == act_reduction_[0] ||
      0.0 == act_reduction_[1] ||
      0.0 == jnt_reduction_[0] ||
      0.0 == jnt_reduction_[1])
  {
    throw TransmissionInterfaceException("Transmission reduction ratios cannot be zero.");
  }
}

inline void FourBarLinkageTransmission::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]);
  *jnt_data.effort[1] = jr[1] * (*act_data.effort[1] * ar[1] - *act_data.effort[0] * ar[0] * jr[0]);
}

inline void FourBarLinkageTransmission::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] / (jr[0] * ar[0]);
  *jnt_data.velocity[1] = (*act_data.velocity[1] / ar[1] - *act_data.velocity[0] / (jr[0] * ar[0])) / jr[1];
}

inline void FourBarLinkageTransmission::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] /(jr[0] * ar[0]) + jnt_offset_[0];
  *jnt_data.position[1] = (*act_data.position[1] / ar[1] - *act_data.position[0] / (jr[0] * ar[0])) / jr[1]
                          + jnt_offset_[1];
}

void FourBarLinkageTransmission::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] && act_data.absolute_position[1] && jnt_data.absolute_position[0] && jnt_data.absolute_position[1]);

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

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

void FourBarLinkageTransmission::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] && act_data.torque_sensor[1] && jnt_data.torque_sensor[0] && jnt_data.torque_sensor[1]);

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

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


inline void FourBarLinkageTransmission::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] / (ar[0] * jr[0]);
  *act_data.effort[1] = (*jnt_data.effort[0] + *jnt_data.effort[1] / jr[1]) / ar[1];
}

inline void FourBarLinkageTransmission::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] * ar[0];
  *act_data.velocity[1] = (*jnt_data.velocity[0] + *jnt_data.velocity[1] * jr[1]) * ar[1];
}

inline void FourBarLinkageTransmission::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] * ar[0];
  *act_data.position[1] = (jnt_pos_off[0] + jnt_pos_off[1] * jr[1]) * ar[1];
}

} // transmission_interface