katana: katana: AbstractKatana.cpp Source File

00001 /*
00002  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
00003  * Copyright (C) 2010  University of Osnabrück
00004  *
00005  * This program is free software; you can redistribute it and/or
00006  * modify it under the terms of the GNU General Public License
00007  * as published by the Free Software Foundation; either version 2
00008  * of the License, or (at your option) any later version.
00009  *
00010  * This program is distributed in the hope that it will be useful,
00011  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  * GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License
00016  * along with this program; if not, write to the Free Software
00017  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00018  *
00019  * AbstractKatana.cpp
00020  *
00021  *  Created on: 20.12.2010
00022  *      Author: Martin Günther <mguenthe@uos.de>
00023  */
00024 
00025 #include <katana/AbstractKatana.h>
00026 
00027 namespace katana
00028 {
00029 
00030 AbstractKatana::AbstractKatana()
00031 {
00032   // names and types for the 5 "real" joints
00033   joint_names_.resize(NUM_JOINTS);
00034   joint_types_.resize(NUM_JOINTS);
00035 
00036   // names and types for the 2 "finger" joints
00037 
00038   gripper_joint_names_.resize(NUM_GRIPPER_JOINTS);
00039   gripper_joint_types_.resize(NUM_GRIPPER_JOINTS);
00040 
00041   // angles and velocities and limits: the 5 "real" joints + gripper
00042   motor_angles_.resize(NUM_MOTORS);
00043   motor_velocities_.resize(NUM_MOTORS);
00044   motor_limits_.resize(NUM_MOTORS + 1);
00045 
00046   /* ********* get parameters ********* */
00047   // ros::NodeHandle pn("~");
00048   ros::NodeHandle n;
00049 
00050   std::string robot_desc_string;
00051 
00052   if (!n.getParam("robot_description", robot_desc_string))
00053   {
00054     ROS_FATAL("Couldn't get a robot_description from the param server");
00055     return;
00056   }
00057 
00058   urdf::Model model;
00059   model.initString(robot_desc_string);
00060 
00061   XmlRpc::XmlRpcValue joint_names;
00062 
00063   // Gets all of the joints
00064   if (!n.getParam("katana_joints", joint_names))
00065   {
00066     ROS_ERROR("No joints given. (namespace: %s)", n.getNamespace().c_str());
00067   }
00068   if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
00069   {
00070     ROS_ERROR("Malformed joint specification.  (namespace: %s)", n.getNamespace().c_str());
00071   }
00072   if (joint_names.size() != (size_t)NUM_JOINTS)
00073   {
00074     ROS_ERROR("Wrong number of joints! was: %d, expected: %zu", joint_names.size(), NUM_JOINTS);
00075   }
00076   for (size_t i = 0; i < NUM_JOINTS; ++i)
00077   {
00078     XmlRpc::XmlRpcValue &name_value = joint_names[i];
00079     if (name_value.getType() != XmlRpc::XmlRpcValue::TypeString)
00080     {
00081       ROS_ERROR("Array of joint names should contain all strings.  (namespace: %s)",
00082           n.getNamespace().c_str());
00083     }
00084 
00085     joint_names_[i] = (std::string)name_value;
00086     joint_types_[i] = urdf::Joint::REVOLUTE; // all of our joints are of type revolute
00087 
00088     motor_limits_[i].joint_name = (std::string)name_value;
00089     motor_limits_[i].min_position = model.getJoint(joint_names_[i])->limits->lower;
00090     motor_limits_[i].max_position = model.getJoint(joint_names_[i])->limits->upper;
00091 
00092     //ROS_INFO("Setting MotorLimit for %s to min: %f - max: %f", motor_limits_[i].joint_name.c_str(), motor_limits_[i].min_position, motor_limits_[i].max_position);
00093 
00094   }
00095 
00096   XmlRpc::XmlRpcValue gripper_joint_names;
00097 
00098   // Gets all of the joints
00099   if (!n.getParam("katana_gripper_joints", gripper_joint_names))
00100   {
00101     ROS_ERROR("No gripper_joints given. (namespace: %s)", n.getNamespace().c_str());
00102   }
00103   if (gripper_joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
00104   {
00105     ROS_ERROR("Malformed gripper_joint specification.  (namespace: %s)", n.getNamespace().c_str());
00106   }
00107   if ((size_t)gripper_joint_names.size() != NUM_GRIPPER_JOINTS)
00108   {
00109     ROS_ERROR("Wrong number of gripper_joints! was: %d, expected: %zu", gripper_joint_names.size(), NUM_GRIPPER_JOINTS);
00110   }
00111   for (size_t i = 0; i < NUM_GRIPPER_JOINTS; ++i)
00112   {
00113     XmlRpc::XmlRpcValue &name_value = gripper_joint_names[i];
00114     if (name_value.getType() != XmlRpc::XmlRpcValue::TypeString)
00115     {
00116       ROS_ERROR("Array of gripper joint names should contain all strings.  (namespace: %s)",
00117           n.getNamespace().c_str());
00118     }
00119 
00120     gripper_joint_names_[i] = (std::string)name_value;
00121     gripper_joint_types_[i] = urdf::Joint::REVOLUTE; // all of our joints are of type revolute
00122 
00123     motor_limits_[NUM_JOINTS + i].joint_name = (std::string)name_value;
00124     motor_limits_[NUM_JOINTS + i].min_position = model.getJoint(gripper_joint_names_[i])->limits->lower;
00125     motor_limits_[NUM_JOINTS + i].max_position = model.getJoint(gripper_joint_names_[i])->limits->upper;
00126 
00127     // ROS_INFO("Setting MotorLimit for %s to min: %f - max: %f", motor_limits_[NUM_JOINTS + i].joint_name.c_str(), motor_limits_[NUM_JOINTS + i].min_position, motor_limits_[NUM_JOINTS + i].max_position);
00128   }
00129 }
00130 
00131 AbstractKatana::~AbstractKatana()
00132 {
00133 }
00134 
00135 void AbstractKatana::freezeRobot()
00136 {
00137   // do nothing (can be overridden)
00138 }
00139 
00140 void AbstractKatana::refreshMotorStatus()
00141 {
00142   // do nothing (can be overridden)
00143 }
00144 
00145 /* ******************************** joints + motors ******************************** */
00146 
00147 int AbstractKatana::getJointIndex(std::string joint_name)
00148 {
00149   for (int i = 0; i < (int)joint_names_.size(); i++)
00150   {
00151     if (joint_names_[i] == joint_name)
00152       return i;
00153   }
00154 
00155   for (int i = 0; i < (int)gripper_joint_names_.size(); i++)
00156   {
00157     if (gripper_joint_names_[i] == joint_name)
00158       return GRIPPER_INDEX;
00159   }
00160 
00161   ROS_ERROR("Joint not found: %s.", joint_name.c_str());
00162   return -1;
00163 }
00164 
00165 std::vector<std::string> AbstractKatana::getJointNames()
00166 {
00167   return joint_names_;
00168 }
00169 
00170 std::vector<int> AbstractKatana::getJointTypes()
00171 {
00172   return joint_types_;
00173 }
00174 
00175 std::vector<std::string> AbstractKatana::getGripperJointNames()
00176 {
00177   return gripper_joint_names_;
00178 }
00179 
00180 std::vector<int> AbstractKatana::getGripperJointTypes()
00181 {
00182   return gripper_joint_types_;
00183 }
00184 
00185 std::vector<double> AbstractKatana::getMotorAngles()
00186 {
00187   return motor_angles_;
00188 }
00189 
00190 std::vector<double> AbstractKatana::getMotorVelocities()
00191 {
00192   return motor_velocities_;
00193 }
00194 
00195 std::vector<arm_navigation_msgs::JointLimits> AbstractKatana::getMotorLimits()
00196 {
00197   return motor_limits_;
00198 }
00199 
00200 double AbstractKatana::getMotorLimitMax(std::string joint_name)
00201 {
00202   for (size_t i = 0; i < motor_limits_.size(); i++)
00203   {
00204     if (motor_limits_[i].joint_name == joint_name)
00205     {
00206       return motor_limits_[i].max_position;
00207     }
00208   }
00209 
00210   return -1;
00211 }
00212 
00213 double AbstractKatana::getMotorLimitMin(std::string joint_name)
00214 {
00215   for (size_t i = 0; i < motor_limits_.size(); i++)
00216   {
00217     if (motor_limits_[i].joint_name == joint_name)
00218     {
00219       return motor_limits_[i].min_position;
00220     }
00221   }
00222 
00223   return -1;
00224 }
00225 
00226 }