$search

joint_force_controller.cpp

Go to the documentation of this file.

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Robert Bosch LLC.
 *  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 Robert Bosch 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.
 *
 *********************************************************************/

#include "pr2_dremel_arm_controller/joint_force_controller.h"
#include "angles/angles.h"
#include "pluginlib/class_list_macros.h"

PLUGINLIB_DECLARE_CLASS(pr2_dremel, JointForceController, pr2_dremel::JointForceController, pr2_controller_interface::Controller)

namespace pr2_dremel {

double sign(double v) {
  if (v < 0.0)
    return -1.0;
  return 1.0;
}

JointForceController::JointForceController()
: initialized_(false), robot_(NULL), last_time_(0), joint_state_(NULL),
  force_control_(true), force_(0.0), position_(0.0), max_velocity_(0.1),
  position_control_limit_(0.03)
{
  reset_velocity_average();
}

JointForceController::~JointForceController()
{
  force_sub_.shutdown();
  position_sub_.shutdown();
}

void JointForceController::reset_velocity_average()
{
  velocity_id_ = 0;
  velocity_sum_ = 0.0;
  for (int i=0; i < velocity_window_; i++) {
    velocities_[i] = 0.0;
  }
}

bool JointForceController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
{
  assert(robot);
  node_ = n;
  robot_ = robot;

  std::string joint_name;
  if (!node_.getParam("joint", joint_name))
  {
    ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
    return false;
  }

  if (!(joint_state_ = robot_->getJointState(joint_name)))
  {
    ROS_ERROR("Could not find joint \"%s\" (namespace: %s)",
              joint_name.c_str(), node_.getNamespace().c_str());
    return false;
  }

  if (!position_pid_controller_.init(ros::NodeHandle(node_, "position/pid")))
    return false;

  if (!velocity_pid_controller_.init(ros::NodeHandle(node_, "velocity/pid")))
    return false;

  node_.param("max_velocity", max_velocity_, 0.1);
  node_.param("position_control_limit", position_control_limit_, 0.03);

  force_sub_ = node_.subscribe<std_msgs::Float64>("force", 1, &JointForceController::forceCB, this);
  position_sub_ = node_.subscribe<std_msgs::Float64>("position", 1, &JointForceController::positionCB, this);
  // vel_pub_ = node_.advertise<std_msgs::Float64>("avg_vel", 10);

  return true;
}

void JointForceController::starting() {
  force_control_ = true;
  force_ = 0.0;
  position_ = joint_state_->position_;
  position_pid_controller_.reset();
  velocity_pid_controller_.reset();

  reset_velocity_average();

  initialized_ = true;
}

void JointForceController::update()
{
  assert(robot_ != NULL);

  velocity_sum_ -= velocities_[velocity_id_];
  velocities_[velocity_id_] = joint_state_->velocity_; 
  velocity_sum_ += velocities_[velocity_id_];
  velocity_id_ = (velocity_id_ + 1)% velocity_window_;

  float average_velocity = velocity_sum_ / velocity_window_;  
  // vel_pub_.publish(average_velocity);
  if (!joint_state_->calibrated_)
    return;

  if (!initialized_)
    starting();

  ros::Time time = robot_->getTime();

  double effort = 0.0;
  if (force_control_) {
    double vel_effort = velocity_effort(max_velocity_ * sign(force_), time);
    if ((fabs(average_velocity) > (0.5 * max_velocity_)) &&
        (sign(average_velocity) == sign(force_))) {
      effort = vel_effort; 
      if ((fabs(vel_effort) > fabs(force_)) && 
          (sign(vel_effort) == sign(force_))) {
        effort = force_;
      }
    } else 
      effort = force_effort(force_);
  } else { // position control
    double pos_error = position_error(position_);
    if (fabs(pos_error) > position_control_limit_)
      effort = velocity_effort(max_velocity_ * sign(-pos_error), time);
    else
      effort = position_effort(position_, time);
  }

  joint_state_->commanded_effort_ = effort;

  last_time_ = time;
}

double JointForceController::force_error(double force)
{
  return (joint_state_->commanded_effort_ - force);
}

double JointForceController::force_effort(double force)
{
  return force;
}

double JointForceController::position_error(double position)
{
  double error(0);

  assert(joint_state_->joint_);

  if(joint_state_->joint_->type == urdf::Joint::REVOLUTE)
  {
    angles::shortest_angular_distance_with_limits(position, joint_state_->position_, joint_state_->joint_->limits->lower, joint_state_->joint_->limits->upper,error);
  }
  else if(joint_state_->joint_->type == urdf::Joint::CONTINUOUS)
  {
    error = angles::shortest_angular_distance(position, joint_state_->position_);
  }
  else //prismatic
  {
    error = joint_state_->position_ - position;
  }

  return error;
}

double JointForceController::position_effort(double position, ros::Time time)
{
  dt_= time - last_time_;

  double error = position_error(position);
  return position_pid_controller_.updatePid(error, dt_);
}

double JointForceController::velocity_effort(double velocity, ros::Time time)
{
  dt_ = time - last_time_;
  double error = joint_state_->velocity_ - velocity;
  return velocity_pid_controller_.updatePid(error, dt_);
}

void JointForceController::forceCB(const std_msgs::Float64ConstPtr& msg)
{
  if (! force_control_) {
    position_pid_controller_.reset();
    velocity_pid_controller_.reset();
    reset_velocity_average();
  }

  force_ = msg->data;
  force_control_ = true;
}


void JointForceController::positionCB(const std_msgs::Float64ConstPtr& msg)
{
  if (force_control_) {
    position_pid_controller_.reset();
    velocity_pid_controller_.reset();
  }

  position_ = msg->data;
  force_control_ = false;
}

}

---

pr2_dremel_arm_controller
Author(s): Christian Bersch (maintained by Benjamin Pitzer)
autogenerated on Sun Mar 3 11:58:25 2013