arm_sim_controller.cpp

Go to the documentation of this file.

//=================================================================================================
// Copyright (c) 2012, Àngel Santamaria-Navarro, IRI-UPC-CSIC
// All rights reserved.

// 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 the Mobile Robotics group,
//       Institut de robòtica i infomàtica industrial (IRI), 
//       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 HOLDER 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.
//=================================================================================================

#include "arm_sim_controller/arm_sim_controller.h"
#include "common/Events.hh"
#include "physics/physics.h"

namespace gazebo {

SimpleArmPlugin::SimpleArmPlugin()
{
}
SimpleArmPlugin::~SimpleArmPlugin()
{
  event::Events::DisconnectWorldUpdateStart(updateConnection_);

  node_handle_->shutdown();
  delete node_handle_;
}
// Load the controller
void SimpleArmPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  this->model_ = _model;
 
  // load parameters
  if (!_sdf->HasElement("robotNamespace"))
    namespace_.clear();
  else
    namespace_ = _sdf->GetElement("robotNamespace")->GetValueString();

  if (!_sdf->HasElement("topicName"))
    joints_topic_ = "joints_state";
  else
    joints_topic_ = _sdf->GetElement("topicName")->GetValueString();
    
  if (!_sdf->HasElement("joint1"))
  { //gzerr << "SimpleArm plugin missing <joint1> element\n";
    ROS_FATAL("SimpleArm plugin missing <joint1> element\n");
    return;
  }
  // get all joints
  this->joints_[0] = _model->GetJoint(_sdf->GetElement("joint1")->GetValueString());

  this->jointPIDs_[0] = common::PID(
    _sdf->GetElement("joint1_pid")->GetValueVector3().x,
    _sdf->GetElement("joint1_pid")->GetValueVector3().y,
    _sdf->GetElement("joint1_pid")->GetValueVector3().z,
    _sdf->GetElement("joint1_ilim")->GetValueVector2d().x,
    _sdf->GetElement("joint1_ilim")->GetValueVector2d().y, 
    _sdf->GetElement("joint1_cmdlim")->GetValueVector2d().x,
    _sdf->GetElement("joint1_cmdlim")->GetValueVector2d().y);
  if (!_sdf->HasElement("joint2"))
  { //gzerr << "SimpleArm plugin missing <joint2> element\n";
    ROS_FATAL("SimpleArm plugin missing <joint2> element\n");
    return;
  }
  this->joints_[1] = _model->GetJoint(_sdf->GetElement("joint2")->GetValueString());
  this->jointPIDs_[1] = common::PID(
    _sdf->GetElement("joint2_pid")->GetValueVector3().x,
    _sdf->GetElement("joint2_pid")->GetValueVector3().y,
    _sdf->GetElement("joint2_pid")->GetValueVector3().z,
    _sdf->GetElement("joint2_ilim")->GetValueVector2d().x,
    _sdf->GetElement("joint2_ilim")->GetValueVector2d().y,    
    _sdf->GetElement("joint2_cmdlim")->GetValueVector2d().x,
    _sdf->GetElement("joint2_cmdlim")->GetValueVector2d().y);

  if (!_sdf->HasElement("joint3"))
  { //gzerr << "SimpleArm plugin missing <joint3> element\n";
    ROS_FATAL("SimpleArm plugin missing <joint3> element\n");
    return;
  }
  this->joints_[2] = _model->GetJoint(_sdf->GetElement("joint3")->GetValueString());
  this->jointPIDs_[2] = common::PID(
    _sdf->GetElement("joint3_pid")->GetValueVector3().x,
    _sdf->GetElement("joint3_pid")->GetValueVector3().y,
    _sdf->GetElement("joint3_pid")->GetValueVector3().z,
    _sdf->GetElement("joint3_ilim")->GetValueVector2d().x,
    _sdf->GetElement("joint3_ilim")->GetValueVector2d().y,
    _sdf->GetElement("joint3_cmdlim")->GetValueVector2d().x,
    _sdf->GetElement("joint3_cmdlim")->GetValueVector2d().y);

  if (!_sdf->HasElement("joint4"))
  { //gzerr << "SimpleArm plugin missing <joint4> element\n";  
    ROS_FATAL("SimpleArm plugin missing <joint4> element\n");
    return;
  }
  this->joints_[3] = _model->GetJoint(_sdf->GetElement("joint4")->GetValueString());
  this->jointPIDs_[3] = common::PID(
    _sdf->GetElement("joint4_pid")->GetValueVector3().x,
    _sdf->GetElement("joint4_pid")->GetValueVector3().y,
    _sdf->GetElement("joint4_pid")->GetValueVector3().z,
    _sdf->GetElement("joint4_ilim")->GetValueVector2d().x,
    _sdf->GetElement("joint4_ilim")->GetValueVector2d().y,    
    _sdf->GetElement("joint4_cmdlim")->GetValueVector2d().x,
    _sdf->GetElement("joint4_cmdlim")->GetValueVector2d().y);
  
  if (!_sdf->HasElement("joint5"))
  { //gzerr << "SimpleArm plugin missing <joint5> element\n";
    ROS_FATAL("SimpleArm plugin missing <joint5> element\n");
  }
  
  this->joints_[4] = _model->GetJoint(_sdf->GetElement("joint5")->GetValueString());
  this->jointPIDs_[4] = common::PID(
    _sdf->GetElement("joint5_pid")->GetValueVector3().x,
    _sdf->GetElement("joint5_pid")->GetValueVector3().y,
    _sdf->GetElement("joint5_pid")->GetValueVector3().z,
    _sdf->GetElement("joint5_ilim")->GetValueVector2d().x,
    _sdf->GetElement("joint5_ilim")->GetValueVector2d().y,
    _sdf->GetElement("joint5_cmdlim")->GetValueVector2d().x,
    _sdf->GetElement("joint5_cmdlim")->GetValueVector2d().y);
 
  //ROS subscriber
  node_handle_ = new ros::NodeHandle(namespace_);

  //subscribe command
  if (!joints_topic_.empty())
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::JointState>(this->joints_topic_, 1, boost::bind(&SimpleArmPlugin::JointsCallback, this, _1), ros::VoidPtr(), &callback_queue_);
    this->joints_subscriber_ = node_handle_->subscribe(ops);
  }
   
  Reset(); 
  
  // New Mechanism for Updating every World Cycle
  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->updateConnection_ = event::Events::ConnectWorldUpdateStart(
      boost::bind(&SimpleArmPlugin::Update, this));
}

// Callback
void SimpleArmPlugin::JointsCallback(const sensor_msgs::JointStateConstPtr& _msg)
{
  for (int i = 0; i < NUM_JOINTS; i++)
   {
     jointPositions_[i]=_msg->position[i];
     jointVelocities_[i]=_msg->velocity[i];
     jointMaxEfforts_[i]=_msg->effort[i];
   }
}

// Update
void SimpleArmPlugin::Update()
{
  // Get simulator time
  common::Time currTime = this->model_->GetWorld()->GetSimTime();
  common::Time stepTime = currTime - this->prevUpdateTime_;
  
  // Get new commands/state
  callback_queue_.callAvailable();

  for (int i = 0; i < NUM_JOINTS; i++)
  {
    // ignore everything else, get position and force only
    double pos_target = this->jointPositions_[i];
    double pos_curr = this->joints_[i]->GetAngle(0).GetAsRadian();
    double max_cmd = this->jointMaxEfforts_[i];
 
    double pos_err = pos_curr - pos_target;
 
    double effort_cmd = this->jointPIDs_[i].Update(pos_err, stepTime);
    effort_cmd = effort_cmd > max_cmd ? max_cmd : (effort_cmd < -max_cmd ? -max_cmd : effort_cmd);
    this->joints_[i]->SetForce(0, effort_cmd);
  }
  
  this->prevUpdateTime_ = currTime;
}

// Reset the controller
void SimpleArmPlugin::Reset()
{
  for (int i = 1; i < NUM_JOINTS; i++)
   {
    this->jointPIDs_[i] = common::PID(1, 0.1, 0.01, 0, 0, 3.1416, -3.1416);
    this->jointPositions_[i] = 0.0;
    this->jointVelocities_[i] = 0.0;
    this->jointMaxEfforts_[i] = 10;
   }
}

GZ_REGISTER_MODEL_PLUGIN(SimpleArmPlugin)

}