PositionController.cpp

Go to the documentation of this file.

/************************************************************************
 *  Copyright (C) 2012 Eindhoven University of Technology (TU/e).       *
 *  All rights reserved.                                                *
 ************************************************************************
 *  Redistribution and use in source and binary forms, with or without  *
 *  modification, are permitted provided that the following conditions  *
 *  are met:                                                            *
 *                                                                      *
 *      1.  Redistributions of source code must retain the above        *
 *          copyright notice, this list of conditions and the following *
 *          disclaimer.                                                 *
 *                                                                      *
 *      2.  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.                             *
 *                                                                      *
 *  THIS SOFTWARE IS PROVIDED BY TU/e "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 TU/e 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.                                                             *
 *                                                                      *
 *  The views and conclusions contained in the software and             *
 *  documentation are those of the authors and should not be            *
 *  interpreted as representing official policies, either expressed or  *
 *  implied, of TU/e.                                                   *
 ************************************************************************/

#include "amigo_gazebo/PositionController.h"
#include <pluginlib/class_list_macros.h>

using namespace controller;

PositionController::PositionController() {

}

PositionController::~PositionController() {
    for(std::vector<JointStruct*>::iterator it = joint_structs_.begin(); it != joint_structs_.end(); ++it) {
        JointStruct* joint_struct = *it;
        delete joint_struct->ref_generator_;
        delete joint_struct;
    }
}

void PositionController::referenceCallback(const sensor_msgs::JointState::ConstPtr& msg) {
    if (msg->position.size() != msg->name.size()) {
        ROS_ERROR("[%s] Malformed reference message: number of joint names does not correspond to number of positions.", node_.getNamespace().c_str());
        return;
    }

    if (msg->position.size() != joint_structs_.size()) {
        ROS_ERROR("[%s] Number of received joint references does not correspond to number of controlled joints.", node_.getNamespace().c_str());
        return;
    }

    for (uint i = 0; i < msg->name.size(); i++) {
        std::map<std::string, JointStruct*>::iterator it_jnt = joint_map_.find(msg->name[i]);
        if (it_jnt != joint_map_.end()) {
            JointStruct* joint_struct = it_jnt->second;
            joint_struct->reference_ = std::max(std::min(msg->position[i], joint_struct->max_pos_), joint_struct->min_pos_);
        } else {
            ROS_ERROR("[%s] Unknown joint name in reference: '%s'.", node_.getNamespace().c_str(), msg->name[i].c_str());
        }
    }
}

bool PositionController::init(pr2_mechanism_model::RobotState* robot, ros::NodeHandle& n) {

    bool error = false;

    node_ = n;

    // Gets all of the joints
    XmlRpc::XmlRpcValue joint_names;
    if (!n.getParam("joints", joint_names)) {
        ROS_ERROR("No joints given. (namespace: %s)", n.getNamespace().c_str());
        return false;
    }
    if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray) {
        ROS_ERROR("Malformed joint specification.  (namespace: %s)", n.getNamespace().c_str());
        return false;
    }

    joint_structs_.resize(joint_names.size());

    for (int i = 0; i < joint_names.size(); ++i) {
        if (joint_names[i].getType() != XmlRpc::XmlRpcValue::TypeString) {
            ROS_ERROR("Array of joint names should contain all strings.  (namespace: %s)", n.getNamespace().c_str());
            return false;
        }

        JointStruct* joint_struct = new JointStruct();

        joint_struct->name_ = (std::string)joint_names[i];

        if (!n.getParam("limits/" + joint_struct->name_ + "/min_position", joint_struct->min_pos_)) {
            ROS_ERROR("[%s] Parameter '%s' not set.", n.getNamespace().c_str(), ("limits/" + joint_struct->name_ + "/min_position").c_str());
            error = true;
        }
        if (!n.getParam("limits/" + joint_struct->name_ + "/max_position", joint_struct->max_pos_)) {
            ROS_ERROR("[%s] Parameter '%s' not set.", n.getNamespace().c_str(), ("limits/" + joint_struct->name_ + "/max_position").c_str());
            error = true;
        }
        if (!n.getParam("limits/" + joint_struct->name_ + "/max_velocity", joint_struct->max_vel_)) {
            ROS_ERROR("[%s] Parameter '%s' not set.", n.getNamespace().c_str(), ("limits/" + joint_struct->name_ + "/max_velocity").c_str());
            error = true;
        }
        if (!n.getParam("limits/" + joint_struct->name_ + "/max_acceleration", joint_struct->max_acc_)) {
            ROS_ERROR("[%s] Parameter '%s' not set.", n.getNamespace().c_str(), ("limits/" + joint_struct->name_ + "/max_acceleration").c_str());
            error = true;
        }
        if (!n.getParam("init_positions/" + joint_struct->name_, joint_struct->reference_)) {
            ROS_ERROR("[%s] Parameter '%s' not set.", n.getNamespace().c_str(), ("init_positions/" + joint_struct->name_).c_str());
            error = true;
        }

        n.param<double>("feedforward/" + joint_struct->name_ + "/mass", joint_struct->mass_ff_, 0);
        n.param<double>("feedforward/" + joint_struct->name_ + "/damping", joint_struct->damp_ff_, 0);
        n.param<double>("feedforward/" + joint_struct->name_ + "/gravity", joint_struct->grav_ff_, 0);

        joint_struct->joint_state_ = robot->getJointState(joint_struct->name_);
        if (!joint_struct->joint_state_) {
            ROS_ERROR("[%s] Could not find joint named '%s' in robot state", n.getNamespace().c_str(), joint_struct->name_.c_str());
            error = true;
        }

        if (!joint_struct->pid_controller_.init(ros::NodeHandle(n, "gains/" + joint_struct->name_))){
            ROS_ERROR("[%s] Could not construct PID controller for %s", n.getNamespace().c_str(), joint_struct->name_.c_str());
            error = true;
        }

        joint_struct->ref_generator_ = new RefGenerator(0, joint_struct->max_vel_, joint_struct->max_acc_);

        joint_structs_[i] = joint_struct;

        joint_map_[joint_struct->name_] = joint_struct;
    }

    if (error) {
        return false;
    }

    references_sub_ = n.subscribe("references", 1000, &PositionController::referenceCallback, this);
    measurements_pub_ = n.advertise<sensor_msgs::JointState>("measurements", 50);

    // copy robot pointer so we can access time
    robot_ = robot;

    return true;
}

void PositionController::starting() {
    for (uint i = 0; i < joint_structs_.size(); i++) {
        joint_structs_[i]->pid_controller_.reset();
        joint_structs_[i]->ref_generator_->setCurrentPosition(joint_structs_[i]->joint_state_->position_);
    }

    time_of_last_cycle_ = robot_->getTime();
}

void PositionController::update() {

    ros::Duration dt = robot_->getTime() - time_of_last_cycle_;

    sensor_msgs::JointState joint_state_msg;
    joint_state_msg.header.stamp = ros::Time::now();

    for(std::vector<JointStruct*>::iterator it = joint_structs_.begin(); it != joint_structs_.end(); ++it) {
        JointStruct* joint_struct = *it;

        //generate vel and acc limited trajectory
        joint_struct->ref_generator_->generate(joint_struct->reference_, dt.toSec(), false);

        // compute feedback
        double feedback = joint_struct->pid_controller_.updatePid(
                    joint_struct->joint_state_->position_
                    - joint_struct->ref_generator_->getPositionReference(), dt);

        //compute feedforward
        double feedforward = joint_struct->mass_ff_ * joint_struct->ref_generator_->getAccelerationReference()
                + joint_struct->damp_ff_ * joint_struct->ref_generator_->getVelocityReference()
                + joint_struct->grav_ff_;

        // set control effort
        joint_struct->joint_state_->commanded_effort_ = feedback + feedforward;

        // add state to message
        joint_state_msg.name.push_back(joint_struct->name_);
        joint_state_msg.position.push_back(joint_struct->joint_state_->position_);
        joint_state_msg.velocity.push_back(joint_struct->joint_state_->velocity_);
        joint_state_msg.effort.push_back(joint_struct->joint_state_->measured_effort_);
    }

    //publish current arm joint positions
    measurements_pub_.publish(joint_state_msg);

    time_of_last_cycle_ = robot_->getTime();
}

void PositionController::stopping() {
    references_sub_.shutdown();
    measurements_pub_.shutdown();
}

// Register controller to pluginlib
PLUGINLIB_REGISTER_CLASS(AmigoPositionController, controller::PositionController, pr2_controller_interface::Controller)