amigo_gazebo: amigo_gazebo: PositionController.cpp Source File

00001 
00013 /************************************************************************
00014  *  Copyright (C) 2012 Eindhoven University of Technology (TU/e).       *
00015  *  All rights reserved.                                                *
00016  ************************************************************************
00017  *  Redistribution and use in source and binary forms, with or without  *
00018  *  modification, are permitted provided that the following conditions  *
00019  *  are met:                                                            *
00020  *                                                                      *
00021  *      1.  Redistributions of source code must retain the above        *
00022  *          copyright notice, this list of conditions and the following *
00023  *          disclaimer.                                                 *
00024  *                                                                      *
00025  *      2.  Redistributions in binary form must reproduce the above     *
00026  *          copyright notice, this list of conditions and the following *
00027  *          disclaimer in the documentation and/or other materials      *
00028  *          provided with the distribution.                             *
00029  *                                                                      *
00030  *  THIS SOFTWARE IS PROVIDED BY TU/e "AS IS" AND ANY EXPRESS OR        *
00031  *  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED      *
00032  *  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE  *
00033  *  ARE DISCLAIMED. IN NO EVENT SHALL TU/e OR CONTRIBUTORS BE LIABLE    *
00034  *  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR        *
00035  *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT   *
00036  *  OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;     *
00037  *  OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF       *
00038  *  LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT           *
00039  *  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE   *
00040  *  USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH    *
00041  *  DAMAGE.                                                             *
00042  *                                                                      *
00043  *  The views and conclusions contained in the software and             *
00044  *  documentation are those of the authors and should not be            *
00045  *  interpreted as representing official policies, either expressed or  *
00046  *  implied, of TU/e.                                                   *
00047  ************************************************************************/
00048 
00049 #include "amigo_gazebo/PositionController.h"
00050 #include <pluginlib/class_list_macros.h>
00051 
00052 using namespace controller;
00053 
00054 PositionController::PositionController() {
00055 
00056 }
00057 
00058 PositionController::~PositionController() {
00059     for(std::vector<JointStruct*>::iterator it = joint_structs_.begin(); it != joint_structs_.end(); ++it) {
00060         JointStruct* joint_struct = *it;
00061         delete joint_struct->ref_generator_;
00062         delete joint_struct;
00063     }
00064 }
00065 
00066 void PositionController::referenceCallback(const sensor_msgs::JointState::ConstPtr& msg) {
00067     if (msg->position.size() != msg->name.size()) {
00068         ROS_ERROR("[%s] Malformed reference message: number of joint names does not correspond to number of positions.", node_.getNamespace().c_str());
00069         return;
00070     }
00071 
00072     if (msg->position.size() != joint_structs_.size()) {
00073         ROS_ERROR("[%s] Number of received joint references does not correspond to number of controlled joints.", node_.getNamespace().c_str());
00074         return;
00075     }
00076 
00077     for (uint i = 0; i < msg->name.size(); i++) {
00078         std::map<std::string, JointStruct*>::iterator it_jnt = joint_map_.find(msg->name[i]);
00079         if (it_jnt != joint_map_.end()) {
00080             JointStruct* joint_struct = it_jnt->second;
00081             joint_struct->reference_ = std::max(std::min(msg->position[i], joint_struct->max_pos_), joint_struct->min_pos_);
00082         } else {
00083             ROS_ERROR("[%s] Unknown joint name in reference: '%s'.", node_.getNamespace().c_str(), msg->name[i].c_str());
00084         }
00085     }
00086 }
00087 
00088 bool PositionController::init(pr2_mechanism_model::RobotState* robot, ros::NodeHandle& n) {
00089 
00090     bool error = false;
00091 
00092     node_ = n;
00093 
00094     // Gets all of the joints
00095     XmlRpc::XmlRpcValue joint_names;
00096     if (!n.getParam("joints", joint_names)) {
00097         ROS_ERROR("No joints given. (namespace: %s)", n.getNamespace().c_str());
00098         return false;
00099     }
00100     if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray) {
00101         ROS_ERROR("Malformed joint specification.  (namespace: %s)", n.getNamespace().c_str());
00102         return false;
00103     }
00104 
00105     joint_structs_.resize(joint_names.size());
00106 
00107     for (int i = 0; i < joint_names.size(); ++i) {
00108         if (joint_names[i].getType() != XmlRpc::XmlRpcValue::TypeString) {
00109             ROS_ERROR("Array of joint names should contain all strings.  (namespace: %s)", n.getNamespace().c_str());
00110             return false;
00111         }
00112 
00113         JointStruct* joint_struct = new JointStruct();
00114 
00115         joint_struct->name_ = (std::string)joint_names[i];
00116 
00117         if (!n.getParam("limits/" + joint_struct->name_ + "/min_position", joint_struct->min_pos_)) {
00118             ROS_ERROR("[%s] Parameter '%s' not set.", n.getNamespace().c_str(), ("limits/" + joint_struct->name_ + "/min_position").c_str());
00119             error = true;
00120         }
00121         if (!n.getParam("limits/" + joint_struct->name_ + "/max_position", joint_struct->max_pos_)) {
00122             ROS_ERROR("[%s] Parameter '%s' not set.", n.getNamespace().c_str(), ("limits/" + joint_struct->name_ + "/max_position").c_str());
00123             error = true;
00124         }
00125         if (!n.getParam("limits/" + joint_struct->name_ + "/max_velocity", joint_struct->max_vel_)) {
00126             ROS_ERROR("[%s] Parameter '%s' not set.", n.getNamespace().c_str(), ("limits/" + joint_struct->name_ + "/max_velocity").c_str());
00127             error = true;
00128         }
00129         if (!n.getParam("limits/" + joint_struct->name_ + "/max_acceleration", joint_struct->max_acc_)) {
00130             ROS_ERROR("[%s] Parameter '%s' not set.", n.getNamespace().c_str(), ("limits/" + joint_struct->name_ + "/max_acceleration").c_str());
00131             error = true;
00132         }
00133         if (!n.getParam("init_positions/" + joint_struct->name_, joint_struct->reference_)) {
00134             ROS_ERROR("[%s] Parameter '%s' not set.", n.getNamespace().c_str(), ("init_positions/" + joint_struct->name_).c_str());
00135             error = true;
00136         }
00137 
00138         n.param<double>("feedforward/" + joint_struct->name_ + "/mass", joint_struct->mass_ff_, 0);
00139         n.param<double>("feedforward/" + joint_struct->name_ + "/damping", joint_struct->damp_ff_, 0);
00140         n.param<double>("feedforward/" + joint_struct->name_ + "/gravity", joint_struct->grav_ff_, 0);
00141 
00142         joint_struct->joint_state_ = robot->getJointState(joint_struct->name_);
00143         if (!joint_struct->joint_state_) {
00144             ROS_ERROR("[%s] Could not find joint named '%s' in robot state", n.getNamespace().c_str(), joint_struct->name_.c_str());
00145             error = true;
00146         }
00147 
00148         if (!joint_struct->pid_controller_.init(ros::NodeHandle(n, "gains/" + joint_struct->name_))){
00149             ROS_ERROR("[%s] Could not construct PID controller for %s", n.getNamespace().c_str(), joint_struct->name_.c_str());
00150             error = true;
00151         }
00152 
00153         joint_struct->ref_generator_ = new RefGenerator(0, joint_struct->max_vel_, joint_struct->max_acc_);
00154 
00155         joint_structs_[i] = joint_struct;
00156 
00157         joint_map_[joint_struct->name_] = joint_struct;
00158     }
00159 
00160     if (error) {
00161         return false;
00162     }
00163 
00164     references_sub_ = n.subscribe("references", 1000, &PositionController::referenceCallback, this);
00165     measurements_pub_ = n.advertise<sensor_msgs::JointState>("measurements", 50);
00166 
00167     // copy robot pointer so we can access time
00168     robot_ = robot;
00169 
00170     return true;
00171 }
00172 
00173 void PositionController::starting() {
00174     for (uint i = 0; i < joint_structs_.size(); i++) {
00175         joint_structs_[i]->pid_controller_.reset();
00176         joint_structs_[i]->ref_generator_->setCurrentPosition(joint_structs_[i]->joint_state_->position_);
00177     }
00178 
00179     time_of_last_cycle_ = robot_->getTime();
00180 }
00181 
00182 void PositionController::update() {
00183 
00184     ros::Duration dt = robot_->getTime() - time_of_last_cycle_;
00185 
00186     sensor_msgs::JointState joint_state_msg;
00187     joint_state_msg.header.stamp = ros::Time::now();
00188 
00189     for(std::vector<JointStruct*>::iterator it = joint_structs_.begin(); it != joint_structs_.end(); ++it) {
00190         JointStruct* joint_struct = *it;
00191 
00192         //generate vel and acc limited trajectory
00193         joint_struct->ref_generator_->generate(joint_struct->reference_, dt.toSec(), false);
00194 
00195         // compute feedback
00196         double feedback = joint_struct->pid_controller_.updatePid(
00197                     joint_struct->joint_state_->position_
00198                     - joint_struct->ref_generator_->getPositionReference(), dt);
00199 
00200         //compute feedforward
00201         double feedforward = joint_struct->mass_ff_ * joint_struct->ref_generator_->getAccelerationReference()
00202                 + joint_struct->damp_ff_ * joint_struct->ref_generator_->getVelocityReference()
00203                 + joint_struct->grav_ff_;
00204 
00205         // set control effort
00206         joint_struct->joint_state_->commanded_effort_ = feedback + feedforward;
00207 
00208         // add state to message
00209         joint_state_msg.name.push_back(joint_struct->name_);
00210         joint_state_msg.position.push_back(joint_struct->joint_state_->position_);
00211         joint_state_msg.velocity.push_back(joint_struct->joint_state_->velocity_);
00212         joint_state_msg.effort.push_back(joint_struct->joint_state_->measured_effort_);
00213     }
00214 
00215     //publish current arm joint positions
00216     measurements_pub_.publish(joint_state_msg);
00217 
00218     time_of_last_cycle_ = robot_->getTime();
00219 }
00220 
00221 void PositionController::stopping() {
00222     references_sub_.shutdown();
00223     measurements_pub_.shutdown();
00224 }
00225 
00226 // Register controller to pluginlib
00227 PLUGINLIB_REGISTER_CLASS(AmigoPositionController, controller::PositionController, pr2_controller_interface::Controller)