kuka_rsi_hw_interface: rsi_state.h Source File

Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2014 Norwegian University of Science and Technology
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Univ of CO, Boulder nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /*
00036  * Author: Lars Tingelstad <lars.tingelstad@ntnu.no>
00037 */
00038 
00039 #ifndef KUKA_RSI_HW_INTERFACE_RSI_STATE_
00040 #define KUKA_RSI_HW_INTERFACE_RSI_STATE_
00041 
00042 #include <string>
00043 #include <tinyxml.h>
00044 
00045 namespace kuka_rsi_hw_interface
00046 {
00047 
00048 class RSIState
00049 {
00050 
00051 private:
00052   std::string xml_doc_;
00053 
00054 public:
00055   RSIState() :
00056     positions(6, 0.0),
00057     initial_positions(6, 0.0),
00058     cart_position(6, 0.0),
00059     initial_cart_position(6, 0.0)
00060   {
00061     xml_doc_.resize(1024);
00062   }
00063 
00064   RSIState(std::string xml_doc);
00065   // AIPOS
00066   std::vector<double> positions;
00067   // ASPos
00068   std::vector<double> initial_positions;
00069   // RIst
00070   std::vector<double> cart_position;
00071   // RSol
00072   std::vector<double> initial_cart_position;
00073   // IPOC
00074   unsigned long long ipoc;
00075 
00076 };
00077 
00078 RSIState::RSIState(std::string xml_doc) :
00079   xml_doc_(xml_doc),
00080   positions(6, 0.0),
00081   initial_positions(6, 0.0),
00082   cart_position(6, 0.0),
00083   initial_cart_position(6, 0.0)
00084 {
00085   // Parse message from robot
00086   TiXmlDocument bufferdoc;
00087   bufferdoc.Parse(xml_doc_.c_str());
00088   // Get the Rob node:
00089   TiXmlElement* rob = bufferdoc.FirstChildElement("Rob");
00090   // Extract axis specific actual position
00091   TiXmlElement* AIPos_el = rob->FirstChildElement("AIPos");
00092   AIPos_el->Attribute("A1", &positions[0]);
00093   AIPos_el->Attribute("A2", &positions[1]);
00094   AIPos_el->Attribute("A3", &positions[2]);
00095   AIPos_el->Attribute("A4", &positions[3]);
00096   AIPos_el->Attribute("A5", &positions[4]);
00097   AIPos_el->Attribute("A6", &positions[5]);
00098   // Extract axis specific setpoint position
00099   TiXmlElement* ASPos_el = rob->FirstChildElement("ASPos");
00100   ASPos_el->Attribute("A1", &initial_positions[0]);
00101   ASPos_el->Attribute("A2", &initial_positions[1]);
00102   ASPos_el->Attribute("A3", &initial_positions[2]);
00103   ASPos_el->Attribute("A4", &initial_positions[3]);
00104   ASPos_el->Attribute("A5", &initial_positions[4]);
00105   ASPos_el->Attribute("A6", &initial_positions[5]);
00106   // Extract cartesian actual position
00107   TiXmlElement* RIst_el = rob->FirstChildElement("RIst");
00108   RIst_el->Attribute("X", &cart_position[0]);
00109   RIst_el->Attribute("Y", &cart_position[1]);
00110   RIst_el->Attribute("Z", &cart_position[2]);
00111   RIst_el->Attribute("A", &cart_position[3]);
00112   RIst_el->Attribute("B", &cart_position[4]);
00113   RIst_el->Attribute("C", &cart_position[5]);
00114   // Extract cartesian actual position
00115   TiXmlElement* RSol_el = rob->FirstChildElement("RSol");
00116   RSol_el->Attribute("X", &initial_cart_position[0]);
00117   RSol_el->Attribute("Y", &initial_cart_position[1]);
00118   RSol_el->Attribute("Z", &initial_cart_position[2]);
00119   RSol_el->Attribute("A", &initial_cart_position[3]);
00120   RSol_el->Attribute("B", &initial_cart_position[4]);
00121   RSol_el->Attribute("C", &initial_cart_position[5]);
00122   // Get the IPOC timestamp
00123   TiXmlElement* ipoc_el = rob->FirstChildElement("IPOC");
00124   ipoc = std::stoull(ipoc_el->FirstChild()->Value());
00125 }
00126 
00127 } // namespace kuka_rsi_hw_interface
00128 
00129 #endif