joint_0_transmission.cpp

Go to the documentation of this file.

/*
* Copyright 2011 Shadow Robot Company Ltd.
*
* This program is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the Free
* Software Foundation version 2 of the License.
*
* This program is distributed in the hope that it will be useful, but WITHOUT
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
* more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include "sr_mechanism_model/joint_0_transmission.hpp"
#include <string>

using std::string;
using sr_actuator::SrMotorActuator;
using ros_ethercat_model::RobotState;
using ros_ethercat_model::Transmission;

PLUGINLIB_EXPORT_CLASS(sr_mechanism_model::J0Transmission, Transmission)

namespace sr_mechanism_model
{

  bool J0Transmission::initXml(tinyxml2::XMLElement *elt, RobotState *robot)
  {
    if (!SimpleTransmission::initXml(elt, robot))
    {
      return false;
    }

    string joint2_name = joint_->joint_->name;
    joint2_name[joint2_name.size() - 1] = '2';
    joint2_ = robot->getJointState(joint2_name);

    return true;
  }

  void J0Transmission::propagatePosition()
  {
    // the size is either 2 or 0 when the joint hasn't been updated yet
    // (joint 0 is composed of the 2 calibrated values: joint 1 and joint 2)

    SrMotorActuator *act = static_cast<SrMotorActuator *>(actuator_);
    size_t size = act->motor_state_.calibrated_sensor_values_.size();
    if (size == 2)
    {
      ROS_DEBUG_STREAM("READING pos " << act->state_.position_
                       << " J1 " << act->motor_state_.calibrated_sensor_values_[0]
                       << " J2 " << act->motor_state_.calibrated_sensor_values_[1]);

      joint_->position_ = act->motor_state_.calibrated_sensor_values_[0];
      joint2_->position_ = act->motor_state_.calibrated_sensor_values_[1];

      joint_->velocity_ = act->state_.velocity_ / 2.0;
      joint2_->velocity_ = act->state_.velocity_ / 2.0;

      joint_->effort_ = act->state_.last_measured_effort_;
      joint2_->effort_ = act->state_.last_measured_effort_;
    }
    else if (size == 0)
    {
      ROS_DEBUG_STREAM("READING pos from Gazebo " << act->state_.position_
                       << " J1 " << act->state_.position_ / 2.0
                       << " J2 " << act->state_.position_ / 2.0);

      // use a real formula for the coupling??
      // GAZEBO
      joint_->position_ = act->state_.position_ / 2.0;
      joint2_->position_ = act->state_.position_ / 2.0;

      joint_->velocity_ = act->state_.velocity_ / 2.0;
      joint2_->velocity_ = act->state_.velocity_ / 2.0;

      joint_->effort_ = act->state_.last_measured_effort_ / 2.0;
      joint2_->effort_ = act->state_.last_measured_effort_ / 2.0;
    }
  }

}  // namespace sr_mechanism_model

/* For the emacs weenies in the crowd.
Local Variables:
   c-basic-offset: 2
End:
 */