Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • sw/simox/simox
  • uwkce_singer/simox
2 results
Show changes
Hide whitespace changes
Inline Side-by-side
Showing
with 0 additions and 2728 deletions
3rdParty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/twist.cpp deleted 100644 → 0
View file @ 2e754261
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* 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 Willow Garage 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.
*********************************************************************/
/* Author: John Hsu */
#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/twist.h>
#include <fstream>
#include <sstream>
#include <boost/lexical_cast.hpp>
#include <algorithm>
#include <tinyxml.h>
#include <console_bridge/console.h>
namespace urdf{
bool parseTwist(Twist &twist, TiXmlElement* xml)
{
twist.clear();
if (xml)
{
const char* linear_char = xml->Attribute("linear");
if (linear_char != NULL)
{
try {
twist.linear.init(linear_char);
}
catch (ParseError &e) {
twist.linear.clear();
logError("Malformed linear string [%s]: %s", linear_char, e.what());
return false;
}
}
const char* angular_char = xml->Attribute("angular");
if (angular_char != NULL)
{
try {
twist.angular.init(angular_char);
}
catch (ParseError &e) {
twist.angular.clear();
logError("Malformed angular [%s]: %s", angular_char, e.what());
return false;
}
}
}
return true;
}
}
3rdParty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_model_state.cpp deleted 100644 → 0
View file @ 2e754261
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* 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 Willow Garage 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.
*********************************************************************/
/* Author: John Hsu */
#include <urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/model_state.h>
#include <fstream>
#include <sstream>
#include <boost/lexical_cast.hpp>
#include <algorithm>
#include <tinyxml.h>
#include <console_bridge/console.h>
namespace urdf{
bool parseModelState(ModelState &ms, TiXmlElement* config)
{
ms.clear();
const char *name_char = config->Attribute("name");
if (!name_char)
{
logError("No name given for the model_state.");
return false;
}
ms.name = std::string(name_char);
const char *time_stamp_char = config->Attribute("time_stamp");
if (time_stamp_char)
{
try {
double sec = boost::lexical_cast<double>(time_stamp_char);
ms.time_stamp.set(sec);
}
catch (boost::bad_lexical_cast &e) {
logError("Parsing time stamp [%s] failed: %s", time_stamp_char, e.what());
return false;
}
}
TiXmlElement *joint_state_elem = config->FirstChildElement("joint_state");
if (joint_state_elem)
{
boost::shared_ptr<JointState> joint_state;
joint_state.reset(new JointState());
const char *joint_char = joint_state_elem->Attribute("joint");
if (joint_char)
joint_state->joint = std::string(joint_char);
else
{
logError("No joint name given for the model_state.");
return false;
}
// parse position
const char *position_char = joint_state_elem->Attribute("position");
if (position_char)
{
std::vector<std::string> pieces;
boost::split( pieces, position_char, boost::is_any_of(" "));
for (unsigned int i = 0; i < pieces.size(); ++i){
if (pieces[i] != ""){
try {
joint_state->position.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
}
catch (boost::bad_lexical_cast &e) {
throw ParseError("position element ("+ pieces[i] +") is not a valid float");
}
}
}
}
// parse velocity
const char *velocity_char = joint_state_elem->Attribute("velocity");
if (velocity_char)
{
std::vector<std::string> pieces;
boost::split( pieces, velocity_char, boost::is_any_of(" "));
for (unsigned int i = 0; i < pieces.size(); ++i){
if (pieces[i] != ""){
try {
joint_state->velocity.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
}
catch (boost::bad_lexical_cast &e) {
throw ParseError("velocity element ("+ pieces[i] +") is not a valid float");
}
}
}
}
// parse effort
const char *effort_char = joint_state_elem->Attribute("effort");
if (effort_char)
{
std::vector<std::string> pieces;
boost::split( pieces, effort_char, boost::is_any_of(" "));
for (unsigned int i = 0; i < pieces.size(); ++i){
if (pieces[i] != ""){
try {
joint_state->effort.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
}
catch (boost::bad_lexical_cast &e) {
throw ParseError("effort element ("+ pieces[i] +") is not a valid float");
}
}
}
}
// add to vector
ms.joint_states.push_back(joint_state);
}
};
}
3rdParty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/urdf_sensor.cpp deleted 100644 → 0
View file @ 2e754261
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* 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 Willow Garage 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.
*********************************************************************/
/* Author: John Hsu */
#include <urdf/urdfdom_headers/urdf_sensor/include/urdf_sensor/sensor.h>
#include <fstream>
#include <sstream>
#include <boost/lexical_cast.hpp>
#include <algorithm>
#include <tinyxml.h>
#include <console_bridge/console.h>
namespace urdf{
bool parsePose(Pose &pose, TiXmlElement* xml);
bool parseCamera(Camera &camera, TiXmlElement* config)
{
camera.clear();
camera.type = VisualSensor::CAMERA;
TiXmlElement *image = config->FirstChildElement("image");
if (image)
{
const char* width_char = image->Attribute("width");
if (width_char)
{
try
{
camera.width = boost::lexical_cast<unsigned int>(width_char);
}
catch (boost::bad_lexical_cast &e)
{
logError("Camera image width [%s] is not a valid int: %s", width_char, e.what());
return false;
}
}
else
{
logError("Camera sensor needs an image width attribute");
return false;
}
const char* height_char = image->Attribute("height");
if (height_char)
{
try
{
camera.height = boost::lexical_cast<unsigned int>(height_char);
}
catch (boost::bad_lexical_cast &e)
{
logError("Camera image height [%s] is not a valid int: %s", height_char, e.what());
return false;
}
}
else
{
logError("Camera sensor needs an image height attribute");
return false;
}
const char* format_char = image->Attribute("format");
if (format_char)
camera.format = std::string(format_char);
else
{
logError("Camera sensor needs an image format attribute");
return false;
}
const char* hfov_char = image->Attribute("hfov");
if (hfov_char)
{
try
{
camera.hfov = boost::lexical_cast<double>(hfov_char);
}
catch (boost::bad_lexical_cast &e)
{
logError("Camera image hfov [%s] is not a valid float: %s", hfov_char, e.what());
return false;
}
}
else
{
logError("Camera sensor needs an image hfov attribute");
return false;
}
const char* near_char = image->Attribute("near");
if (near_char)
{
try
{
camera.near = boost::lexical_cast<double>(near_char);
}
catch (boost::bad_lexical_cast &e)
{
logError("Camera image near [%s] is not a valid float: %s", near_char, e.what());
return false;
}
}
else
{
logError("Camera sensor needs an image near attribute");
return false;
}
const char* far_char = image->Attribute("far");
if (far_char)
{
try
{
camera.far = boost::lexical_cast<double>(far_char);
}
catch (boost::bad_lexical_cast &e)
{
logError("Camera image far [%s] is not a valid float: %s", far_char, e.what());
return false;
}
}
else
{
logError("Camera sensor needs an image far attribute");
return false;
}
}
else
{
logError("Camera sensor has no <image> element");
return false;
}
return true;
}
bool parseRay(Ray &ray, TiXmlElement* config)
{
ray.clear();
ray.type = VisualSensor::RAY;
TiXmlElement *horizontal = config->FirstChildElement("horizontal");
if (horizontal)
{
const char* samples_char = horizontal->Attribute("samples");
if (samples_char)
{
try
{
ray.horizontal_samples = boost::lexical_cast<unsigned int>(samples_char);
}
catch (boost::bad_lexical_cast &e)
{
logError("Ray horizontal samples [%s] is not a valid float: %s", samples_char, e.what());
return false;
}
}
const char* resolution_char = horizontal->Attribute("resolution");
if (resolution_char)
{
try
{
ray.horizontal_resolution = boost::lexical_cast<double>(resolution_char);
}
catch (boost::bad_lexical_cast &e)
{
logError("Ray horizontal resolution [%s] is not a valid float: %s", resolution_char, e.what());
return false;
}
}
const char* min_angle_char = horizontal->Attribute("min_angle");
if (min_angle_char)
{
try
{
ray.horizontal_min_angle = boost::lexical_cast<double>(min_angle_char);
}
catch (boost::bad_lexical_cast &e)
{
logError("Ray horizontal min_angle [%s] is not a valid float: %s", min_angle_char, e.what());
return false;
}
}
const char* max_angle_char = horizontal->Attribute("max_angle");
if (max_angle_char)
{
try
{
ray.horizontal_max_angle = boost::lexical_cast<double>(max_angle_char);
}
catch (boost::bad_lexical_cast &e)
{
logError("Ray horizontal max_angle [%s] is not a valid float: %s", max_angle_char, e.what());
return false;
}
}
}
TiXmlElement *vertical = config->FirstChildElement("vertical");
if (vertical)
{
const char* samples_char = vertical->Attribute("samples");
if (samples_char)
{
try
{
ray.vertical_samples = boost::lexical_cast<unsigned int>(samples_char);
}
catch (boost::bad_lexical_cast &e)
{
logError("Ray vertical samples [%s] is not a valid float: %s", samples_char, e.what());
return false;
}
}
const char* resolution_char = vertical->Attribute("resolution");
if (resolution_char)
{
try
{
ray.vertical_resolution = boost::lexical_cast<double>(resolution_char);
}
catch (boost::bad_lexical_cast &e)
{
logError("Ray vertical resolution [%s] is not a valid float: %s", resolution_char, e.what());
return false;
}
}
const char* min_angle_char = vertical->Attribute("min_angle");
if (min_angle_char)
{
try
{
ray.vertical_min_angle = boost::lexical_cast<double>(min_angle_char);
}
catch (boost::bad_lexical_cast &e)
{
logError("Ray vertical min_angle [%s] is not a valid float: %s", min_angle_char, e.what());
return false;
}
}
const char* max_angle_char = vertical->Attribute("max_angle");
if (max_angle_char)
{
try
{
ray.vertical_max_angle = boost::lexical_cast<double>(max_angle_char);
}
catch (boost::bad_lexical_cast &e)
{
logError("Ray vertical max_angle [%s] is not a valid float: %s", max_angle_char, e.what());
return false;
}
}
}
}
boost::shared_ptr<VisualSensor> parseVisualSensor(TiXmlElement *g)
{
boost::shared_ptr<VisualSensor> visual_sensor;
// get sensor type
TiXmlElement *sensor_xml;
if (g->FirstChildElement("camera"))
{
Camera *camera = new Camera();
visual_sensor.reset(camera);
sensor_xml = g->FirstChildElement("camera");
if (!parseCamera(*camera, sensor_xml))
visual_sensor.reset();
}
else if (g->FirstChildElement("ray"))
{
Ray *ray = new Ray();
visual_sensor.reset(ray);
sensor_xml = g->FirstChildElement("ray");
if (!parseRay(*ray, sensor_xml))
visual_sensor.reset();
}
else
{
logError("No know sensor types [camera|ray] defined in <sensor> block");
}
return visual_sensor;
}
bool parseSensor(Sensor &sensor, TiXmlElement* config)
{
sensor.clear();
const char *name_char = config->Attribute("name");
if (!name_char)
{
logError("No name given for the sensor.");
return false;
}
sensor.name = std::string(name_char);
// parse parent_link_name
const char *parent_link_name_char = config->Attribute("parent_link_name");
if (!parent_link_name_char)
{
logError("No parent_link_name given for the sensor.");
return false;
}
sensor.parent_link_name = std::string(parent_link_name_char);
// parse origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parsePose(sensor.origin, o))
return false;
}
// parse sensor
sensor.sensor = parseVisualSensor(config);
return true;
}
}
3rdParty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/src/world.cpp deleted 100644 → 0
View file @ 2e754261
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* 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 Willow Garage 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.
*********************************************************************/
/* Author: Wim Meeussen */
#include <urdf/urdfdom_headers/urdf_world/include/urdf_world/world.h>
#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h>
#include <urdf_parser/urdf_parser.h>
#include <fstream>
#include <sstream>
#include <boost/lexical_cast.hpp>
#include <algorithm>
#include <tinyxml.h>
#include <console_bridge/console.h>
namespace urdf{
bool parseWorld(World &world, TiXmlElement* config)
{
// to be implemented
return true;
}
bool exportWorld(World &world, TiXmlElement* xml)
{
TiXmlElement * world_xml = new TiXmlElement("world");
world_xml->SetAttribute("name", world.name);
// to be implemented
// exportModels(*world.models, world_xml);
xml->LinkEndChild(world_xml);
return true;
}
}
3rdParty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/test/memtest.cpp deleted 100644 → 0
View file @ 2e754261
#include "urdf_parser/urdf_parser.h"
#include <fstream>
#include <iostream>
int main(int argc, char** argv){
while (true){
std::string xml_string;
std::fstream xml_file(argv[1], std::fstream::in);
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
urdf::parseURDF(xml_string);
}
}
3rdParty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/LICENSE deleted 100644 → 0
View file @ 2e754261
Software License Agreement (Apache License)
Copyright 2011 John Hsu
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
3rdParty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/README.txt deleted 100644 → 0
View file @ 2e754261
The URDF (U-Robot Description Format) headers
provides core data structure headers for URDF.
For now, the details of the URDF specifications reside on
http://ros.org/wiki/urdf
3rdParty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h deleted 100644 → 0
View file @ 2e754261
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* 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 Willow Garage 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.
*********************************************************************/
// URDF exceptions
#ifndef URDF_INTERFACE_EXCEPTION_H_
#define URDF_INTERFACE_EXCEPTION_H_
#include <string>
#include <stdexcept>
namespace urdf
{
class ParseError: public std::runtime_error
{
public:
ParseError(const std::string &error_msg) : std::runtime_error(error_msg) {};
};
}
#endif
3rdParty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/color.h deleted 100644 → 0
View file @ 2e754261
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* 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 Willow Garage 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.
*********************************************************************/
/* Author: Josh Faust */
#ifndef URDF_INTERFACE_COLOR_H
#define URDF_INTERFACE_COLOR_H
#include <string>
#include <vector>
#include <math.h>
//#include <boost/algorithm/string.hpp>
//#include <boost/lexical_cast.hpp>
namespace urdf
{
class Color
{
public:
Color() {this->clear();};
float r;
float g;
float b;
float a;
void clear()
{
r = g = b = 0.0f;
a = 1.0f;
}
bool init(const std::string &vector_str)
{
this->clear();
std::vector<std::string> pieces;
std::vector<float> rgba;
boost::split( pieces, vector_str, boost::is_any_of(" "));
for (unsigned int i = 0; i < pieces.size(); ++i)
{
if (!pieces[i].empty())
{
try
{
rgba.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
}
catch (boost::bad_lexical_cast &e)
{
return false;
}
}
}
if (rgba.size() != 4)
{
return false;
}
this->r = rgba[0];
this->g = rgba[1];
this->b = rgba[2];
this->a = rgba[3];
return true;
};
};
}
#endif
3rdParty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/joint.h deleted 100644 → 0
View file @ 2e754261
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* 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 Willow Garage 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.
*********************************************************************/
/* Author: Wim Meeussen */
#ifndef URDF_INTERFACE_JOINT_H
#define URDF_INTERFACE_JOINT_H
#include <string>
#include <vector>
#ifdef URDF_USE_BOOST
#include <boost/shared_ptr.hpp>
#define my_shared_ptr my_shared_ptr
#else
#include <urdf/boost_replacement/shared_ptr.h>
#endif
#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h>
namespace urdf{
class Link;
class JointDynamics
{
public:
JointDynamics() { this->clear(); };
double damping;
double friction;
void clear()
{
damping = 0;
friction = 0;
};
};
class JointLimits
{
public:
JointLimits() { this->clear(); };
double lower;
double upper;
double effort;
double velocity;
void clear()
{
lower = 0;
upper = 0;
effort = 0;
velocity = 0;
};
};
/// \brief Parameters for Joint Safety Controllers
class JointSafety
{
public:
/// clear variables on construction
JointSafety() { this->clear(); };
///
/// IMPORTANT: The safety controller support is very much PR2 specific, not intended for generic usage.
///
/// Basic safety controller operation is as follows
///
/// current safety controllers will take effect on joints outside the position range below:
///
/// position range: [JointSafety::soft_lower_limit + JointLimits::velocity / JointSafety::k_position,
/// JointSafety::soft_uppper_limit - JointLimits::velocity / JointSafety::k_position]
///
/// if (joint_position is outside of the position range above)
/// velocity_limit_min = -JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_lower_limit)
/// velocity_limit_max = JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_upper_limit)
/// else
/// velocity_limit_min = -JointLimits::velocity
/// velocity_limit_max = JointLimits::velocity
///
/// velocity range: [velocity_limit_min + JointLimits::effort / JointSafety::k_velocity,
/// velocity_limit_max - JointLimits::effort / JointSafety::k_velocity]
///
/// if (joint_velocity is outside of the velocity range above)
/// effort_limit_min = -JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_min)
/// effort_limit_max = JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_max)
/// else
/// effort_limit_min = -JointLimits::effort
/// effort_limit_max = JointLimits::effort
///
/// Final effort command sent to the joint is saturated by [effort_limit_min,effort_limit_max]
///
/// Please see wiki for more details: http://www.ros.org/wiki/pr2_controller_manager/safety_limits
///
double soft_upper_limit;
double soft_lower_limit;
double k_position;
double k_velocity;
void clear()
{
soft_upper_limit = 0;
soft_lower_limit = 0;
k_position = 0;
k_velocity = 0;
};
};
class JointCalibration
{
public:
JointCalibration() { this->clear(); };
double reference_position;
my_shared_ptr<double> rising, falling;
void clear()
{
reference_position = 0;
};
};
class JointMimic
{
public:
JointMimic() { this->clear(); };
double offset;
double multiplier;
std::string joint_name;
void clear()
{
offset = 0.0;
multiplier = 0.0;
joint_name.clear();
};
};
class Joint
{
public:
Joint() { this->clear(); };
std::string name;
enum
{
UNKNOWN, REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR, FIXED
} type;
/// \brief type_ meaning of axis_
/// ------------------------------------------------------
/// UNKNOWN unknown type
/// REVOLUTE rotation axis
/// PRISMATIC translation axis
/// FLOATING N/A
/// PLANAR plane normal axis
/// FIXED N/A
Vector3 axis;
/// child Link element
/// child link frame is the same as the Joint frame
std::string child_link_name;
/// parent Link element
/// origin specifies the transform from Parent Link to Joint Frame
std::string parent_link_name;
/// transform from Parent Link frame to Joint frame
Pose parent_to_joint_origin_transform;
/// Joint Dynamics
my_shared_ptr<JointDynamics> dynamics;
/// Joint Limits
my_shared_ptr<JointLimits> limits;
/// Unsupported Hidden Feature
my_shared_ptr<JointSafety> safety;
/// Unsupported Hidden Feature
my_shared_ptr<JointCalibration> calibration;
/// Option to Mimic another Joint
my_shared_ptr<JointMimic> mimic;
void clear()
{
this->axis.clear();
this->child_link_name.clear();
this->parent_link_name.clear();
this->parent_to_joint_origin_transform.clear();
this->dynamics.reset(0);
this->limits.reset(0);
this->safety.reset(0);
this->calibration.reset(0);
this->type = UNKNOWN;
};
};
}
#endif
3rdParty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h deleted 100644 → 0
View file @ 2e754261
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* 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 Willow Garage 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.
*********************************************************************/
/* Author: Wim Meeussen */
#ifndef URDF_INTERFACE_LINK_H
#define URDF_INTERFACE_LINK_H
#include <string>
#include <vector>
#include <map>
#ifdef URDF_USE_BOOST
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#else
#include <urdf/boost_replacement/shared_ptr.h>
#endif
#include "joint.h"
#include "color.h"
#include "pose.h"
namespace urdf{
class Geometry
{
public:
enum {SPHERE, BOX, CYLINDER, MESH} type;
virtual ~Geometry(void)
{
}
};
class Sphere : public Geometry
{
public:
Sphere() { this->clear(); type = SPHERE; };
double radius;
void clear()
{
radius = 0;
};
};
class Box : public Geometry
{
public:
Box() { this->clear(); type = BOX; }
Vector3 dim;
void clear()
{
this->dim.clear();
};
};
class Cylinder : public Geometry
{
public:
Cylinder() { this->clear(); type = CYLINDER; };
double length;
double radius;
void clear()
{
length = 0;
radius = 0;
};
};
class Mesh : public Geometry
{
public:
Mesh() { this->clear(); type = MESH; };
std::string filename;
Vector3 scale;
void clear()
{
filename.clear();
// default scale
scale.x = 1;
scale.y = 1;
scale.z = 1;
};
};
class Material
{
public:
Material() { this->clear(); };
std::string name;
std::string texture_filename;
Color color;
void clear()
{
color.clear();
texture_filename.clear();
name.clear();
};
};
class Inertial
{
public:
Inertial() { this->clear(); };
Pose origin;
double mass;
double ixx,ixy,ixz,iyy,iyz,izz;
void clear()
{
origin.clear();
mass = 0;
ixx = ixy = ixz = iyy = iyz = izz = 0;
};
};
class Visual
{
public:
Visual() { this->clear(); };
Pose origin;
my_shared_ptr<Geometry> geometry;
std::string material_name;
my_shared_ptr<Material> material;
void clear()
{
origin.clear();
material_name.clear();
material.reset(0);
geometry.reset(0);
name.clear();
};
std::string name;
};
class Collision
{
public:
Collision() { this->clear(); };
Pose origin;
my_shared_ptr<Geometry> geometry;
void clear()
{
origin.clear();
geometry.reset(0);
name.clear();
};
std::string name;
};
class Link
{
public:
Link() { this->clear(); };
std::string name;
/// inertial element
my_shared_ptr<Inertial> inertial;
/// visual element
my_shared_ptr<Visual> visual;
/// collision element
my_shared_ptr<Collision> collision;
/// if more than one collision element is specified, all collision elements are placed in this array (the collision member points to the first element of the array)
std::vector<my_shared_ptr<Collision> > collision_array;
/// if more than one visual element is specified, all visual elements are placed in this array (the visual member points to the first element of the array)
std::vector<my_shared_ptr<Visual> > visual_array;
/// Parent Joint element
/// explicitly stating "parent" because we want directional-ness for tree structure
/// every link can have one parent
my_shared_ptr<Joint> parent_joint;
std::vector<my_shared_ptr<Joint> > child_joints;
std::vector<my_shared_ptr<Link> > child_links;
mutable int m_link_index;
const Link* getParent() const
{return parent_link_;}
void setParent(const my_shared_ptr<Link> &parent)
{
parent_link_ = parent.get();
}
void clear()
{
this->name.clear();
this->inertial.reset(0);
this->visual.reset(0);
this->collision.reset(0);
this->parent_joint.reset(0);
this->child_joints.clear();
this->child_links.clear();
this->collision_array.clear();
this->visual_array.clear();
this->m_link_index=-1;
this->parent_link_ = NULL;
};
private:
// boost::weak_ptr<Link> parent_link_;
const Link* parent_link_;
};
}
#endif
3rdParty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h deleted 100644 → 0
View file @ 2e754261
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* 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 Willow Garage 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.
*********************************************************************/
/* Author: Wim Meeussen */
#ifndef URDF_INTERFACE_MODEL_H
#define URDF_INTERFACE_MODEL_H
#include <string>
#include <map>
//#include <boost/function.hpp>
#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h>
#include <stdio.h> //printf
#include <urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h>
namespace urdf {
class ModelInterface
{
public:
my_shared_ptr<const Link> getRoot(void) const{return this->root_link_;};
my_shared_ptr<const Link> getLink(const std::string& name) const
{
my_shared_ptr<const Link> ptr;
if (this->links_.find(name) == this->links_.end())
ptr.reset(0);
else
ptr = this->links_.find(name)->second;
return ptr;
};
my_shared_ptr<const Joint> getJoint(const std::string& name) const
{
my_shared_ptr<const Joint> ptr;
if (this->joints_.find(name) == this->joints_.end())
ptr.reset(0);
else
ptr = this->joints_.find(name)->second;
return ptr;
};
const std::string& getName() const {return name_;};
void getLinks(std::vector<my_shared_ptr<Link> >& links) const
{
for (std::map<std::string,my_shared_ptr<Link> >::const_iterator link = this->links_.begin();link != this->links_.end(); link++)
{
links.push_back(link->second);
}
};
void clear()
{
m_numLinks=0;
m_numJoints = 0;
name_.clear();
this->links_.clear();
this->joints_.clear();
this->materials_.clear();
this->root_link_.reset(0);
};
/// non-const getLink()
void getLink(const std::string& name,my_shared_ptr<Link> &link) const
{
my_shared_ptr<Link> ptr;
if (this->links_.find(name) == this->links_.end())
ptr.reset(0);
else
ptr = this->links_.find(name)->second;
link = ptr;
};
/// non-const getMaterial()
my_shared_ptr<Material> getMaterial(const std::string& name) const
{
my_shared_ptr<Material> ptr;
if (this->materials_.find(name) == this->materials_.end())
ptr.reset(0);
else
ptr = this->materials_.find(name)->second;
return ptr;
};
void initTree(std::map<std::string, std::string> &parent_link_tree)
{
// loop through all joints, for every link, assign children links and children joints
for (std::map<std::string,my_shared_ptr<Joint> >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++)
{
std::string parent_link_name = joint->second->parent_link_name;
std::string child_link_name = joint->second->child_link_name;
if (parent_link_name.empty() || child_link_name.empty())
{
assert(0);
// throw ParseError("Joint [" + joint->second->name + "] is missing a parent and/or child link specification.");
}
else
{
// find child and parent links
my_shared_ptr<Link> child_link, parent_link;
this->getLink(child_link_name, child_link);
if (!child_link)
{
printf("Error: child link [%s] of joint [%s] not found\n", child_link_name.c_str(),joint->first.c_str() );
assert(0);
// throw ParseError("child link [" + child_link_name + "] of joint [" + joint->first + "] not found");
}
this->getLink(parent_link_name, parent_link);
if (!parent_link)
{
assert(0);
/* throw ParseError("parent link [" + parent_link_name + "] of joint [" + joint->first + "] not found. This is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint [" + joint->first + "] from your urdf file, or add \"<link name=\"" + parent_link_name + "\" />\" to your urdf file.");
*/}
//set parent link for child link
child_link->setParent(parent_link);
//set parent joint for child link
child_link->parent_joint = joint->second;
//set child joint for parent link
parent_link->child_joints.push_back(joint->second);
//set child link for parent link
parent_link->child_links.push_back(child_link);
// fill in child/parent string map
parent_link_tree[child_link->name] = parent_link_name;
}
}
}
void initRoot(const std::map<std::string, std::string> &parent_link_tree)
{
this->root_link_.reset(0);
// find the links that have no parent in the tree
for (std::map<std::string, my_shared_ptr<Link> >::const_iterator l=this->links_.begin(); l!=this->links_.end(); l++)
{
std::map<std::string, std::string >::const_iterator parent = parent_link_tree.find(l->first);
if (parent == parent_link_tree.end())
{
// store root link
if (!this->root_link_)
{
getLink(l->first, this->root_link_);
}
// we already found a root link
else
{
assert(0);
// throw ParseError("Two root links found: [" + this->root_link_->name + "] and [" + l->first + "]");
}
}
}
if (!this->root_link_)
{
assert(0);
//throw ParseError("No root link found. The robot xml is not a valid tree.");
}
}
/// \brief complete list of Links
std::map<std::string, my_shared_ptr<Link> > links_;
/// \brief complete list of Joints
std::map<std::string, my_shared_ptr<Joint> > joints_;
/// \brief complete list of Materials
std::map<std::string, my_shared_ptr<Material> > materials_;
/// \brief The name of the robot model
std::string name_;
/// \brief The root is always a link (the parent of the tree describing the robot)
my_shared_ptr<Link> root_link_;
int m_numLinks;//includes parent
int m_numJoints;
};
}
#endif
3rdParty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h deleted 100644 → 0
View file @ 2e754261
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* 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 Willow Garage 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.
*********************************************************************/
/* Author: Wim Meeussen */
#ifndef URDF_INTERFACE_POSE_H
#define URDF_INTERFACE_POSE_H
#include <string>
//#include <sstream>
#include <vector>
#include <math.h>
#ifndef M_PI
#define M_PI 3.141592538
#endif //M_PI
#ifdef URDF_USE_BOOST
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#else
#include <urdf/boost_replacement/string_split.h>
#include <urdf/boost_replacement/lexical_cast.h>
#endif //URDF_USE_BOOST
#include <urdf/urdfdom_headers/urdf_exception/include/urdf_exception/exception.h>
#include <assert.h>
namespace urdf{
class Vector3
{
public:
Vector3(double _x,double _y, double _z) {this->x=_x;this->y=_y;this->z=_z;};
Vector3() {this->clear();};
double x;
double y;
double z;
void clear() {this->x=this->y=this->z=0.0;};
void init(const std::string &vector_str)
{
this->clear();
std::vector<std::string> pieces;
std::vector<double> xyz;
boost::split( pieces, vector_str, boost::is_any_of(" "));
for (unsigned int i = 0; i < pieces.size(); ++i){
if (pieces[i] != ""){
try {
xyz.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
}
catch (boost::bad_lexical_cast &e)
{
assert(0);
// throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a vector value)");
}
}
}
if (xyz.size() != 3)
{
assert(0);
// throw ParseError("Parser found " + boost::lexical_cast<std::string>(xyz.size()) + " elements but 3 expected while parsing vector [" + vector_str + "]");
}
this->x = xyz[0];
this->y = xyz[1];
this->z = xyz[2];
}
Vector3 operator+(Vector3 vec)
{
return Vector3(this->x+vec.x,this->y+vec.y,this->z+vec.z);
};
};
class Rotation
{
public:
Rotation(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;};
Rotation() {this->clear();};
void getQuaternion(double &quat_x,double &quat_y,double &quat_z, double &quat_w) const
{
quat_x = this->x;
quat_y = this->y;
quat_z = this->z;
quat_w = this->w;
};
void getRPY(double &roll,double &pitch,double &yaw) const
{
double sqw;
double sqx;
double sqy;
double sqz;
sqx = this->x * this->x;
sqy = this->y * this->y;
sqz = this->z * this->z;
sqw = this->w * this->w;
roll = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz);
double sarg = -2 * (this->x*this->z - this->w*this->y);
pitch = sarg <= -1.0 ? -0.5*M_PI : (sarg >= 1.0 ? 0.5*M_PI : asin(sarg));
yaw = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz);
};
void setFromQuaternion(double quat_x,double quat_y,double quat_z,double quat_w)
{
this->x = quat_x;
this->y = quat_y;
this->z = quat_z;
this->w = quat_w;
this->normalize();
};
void setFromRPY(double roll, double pitch, double yaw)
{
double phi, the, psi;
phi = roll / 2.0;
the = pitch / 2.0;
psi = yaw / 2.0;
this->x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi);
this->y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi);
this->z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi);
this->w = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi);
this->normalize();
};
double x,y,z,w;
void init(const std::string &rotation_str)
{
this->clear();
Vector3 rpy;
rpy.init(rotation_str);
setFromRPY(rpy.x, rpy.y, rpy.z);
}
void clear() { this->x=this->y=this->z=0.0;this->w=1.0; }
void normalize()
{
double s = sqrt(this->x * this->x +
this->y * this->y +
this->z * this->z +
this->w * this->w);
if (s == 0.0)
{
this->x = 0.0;
this->y = 0.0;
this->z = 0.0;
this->w = 1.0;
}
else
{
this->x /= s;
this->y /= s;
this->z /= s;
this->w /= s;
}
};
// Multiplication operator (copied from gazebo)
Rotation operator*( const Rotation &qt ) const
{
Rotation c;
c.x = this->w * qt.x + this->x * qt.w + this->y * qt.z - this->z * qt.y;
c.y = this->w * qt.y - this->x * qt.z + this->y * qt.w + this->z * qt.x;
c.z = this->w * qt.z + this->x * qt.y - this->y * qt.x + this->z * qt.w;
c.w = this->w * qt.w - this->x * qt.x - this->y * qt.y - this->z * qt.z;
return c;
};
/// Rotate a vector using the quaternion
Vector3 operator*(Vector3 vec) const
{
Rotation tmp;
Vector3 result;
tmp.w = 0.0;
tmp.x = vec.x;
tmp.y = vec.y;
tmp.z = vec.z;
tmp = (*this) * (tmp * this->GetInverse());
result.x = tmp.x;
result.y = tmp.y;
result.z = tmp.z;
return result;
};
// Get the inverse of this quaternion
Rotation GetInverse() const
{
Rotation q;
double norm = this->w*this->w+this->x*this->x+this->y*this->y+this->z*this->z;
if (norm > 0.0)
{
q.w = this->w / norm;
q.x = -this->x / norm;
q.y = -this->y / norm;
q.z = -this->z / norm;
}
return q;
};
};
class Pose
{
public:
Pose() { this->clear(); };
Vector3 position;
Rotation rotation;
void clear()
{
this->position.clear();
this->rotation.clear();
};
};
}
#endif
3rdParty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/twist.h deleted 100644 → 0
View file @ 2e754261
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* 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 Willow Garage 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.
*********************************************************************/
/* Author: John Hsu */
#ifndef URDF_TWIST_H
#define URDF_TWIST_H
#include <string>
#include <sstream>
#include <vector>
#include <math.h>
#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/pose.h>
namespace urdf{
class Twist
{
public:
Twist() { this->clear(); };
Vector3 linear;
// Angular velocity represented by Euler angles
Vector3 angular;
void clear()
{
this->linear.clear();
this->angular.clear();
};
};
}
#endif
3rdParty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/model_state.h deleted 100644 → 0
View file @ 2e754261
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* 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 Willow Garage 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.
*********************************************************************/
/* Author: John Hsu */
#ifndef URDF_MODEL_STATE_H
#define URDF_MODEL_STATE_H
#include <string>
#include <vector>
#include <map>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#include "urdf_model/pose.h"
#include <urdf_model/twist.h>
namespace urdf{
class Time
{
public:
Time() { this->clear(); };
void set(double _seconds)
{
this->sec = (int32_t)(floor(_seconds));
this->nsec = (int32_t)(round((_seconds - this->sec) * 1e9));
this->Correct();
};
operator double ()
{
return (static_cast<double>(this->sec) +
static_cast<double>(this->nsec)*1e-9);
};
int32_t sec;
int32_t nsec;
void clear()
{
this->sec = 0;
this->nsec = 0;
};
private:
void Correct()
{
// Make any corrections
if (this->nsec >= 1e9)
{
this->sec++;
this->nsec = (int32_t)(this->nsec - 1e9);
}
else if (this->nsec < 0)
{
this->sec--;
this->nsec = (int32_t)(this->nsec + 1e9);
}
};
};
class JointState
{
public:
JointState() { this->clear(); };
/// joint name
std::string joint;
std::vector<double> position;
std::vector<double> velocity;
std::vector<double> effort;
void clear()
{
this->joint.clear();
this->position.clear();
this->velocity.clear();
this->effort.clear();
}
};
class ModelState
{
public:
ModelState() { this->clear(); };
/// state name must be unique
std::string name;
Time time_stamp;
void clear()
{
this->name.clear();
this->time_stamp.set(0);
this->joint_states.clear();
};
std::vector<boost::shared_ptr<JointState> > joint_states;
};
}
#endif
3rdParty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model_state/include/urdf_model_state/twist.h deleted 100644 → 0
View file @ 2e754261
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* 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 Willow Garage 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.
*********************************************************************/
#ifndef URDF_MODEL_STATE_TWIST_
#define URDF_MODEL_STATE_TWIST_
#warning "Please Use #include <urdf_model/twist.h>"
#include <urdf_model/twist.h>
#endif
3rdParty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_sensor/include/urdf_sensor/sensor.h deleted 100644 → 0
View file @ 2e754261
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* 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 Willow Garage 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.
*********************************************************************/
/* Author: John Hsu */
/* example
<sensor name="my_camera_sensor" update_rate="20">
<origin xyz="0 0 0" rpy="0 0 0"/>
<camera>
<horizontal_hov>1.5708</horizontal_hov>
<image width="640" height="480" format="R8G8B8"/>
<clip near="0.01" far="50.0"/>
</camera>
</sensor>
<sensor name="my_ray_sensor" update_rate="20">
<origin xyz="0 0 0" rpy="0 0 0"/>
<ray>
<scan>
<horizontal samples="100" resolution="1" min_angle="-1.5708" max_angle="1.5708"/>
<vertical samples="1" resolution="1" min_angle="0" max_angle="0"/>
</scan>
</ray>
</sensor>
*/
#ifndef URDF_SENSOR_H
#define URDF_SENSOR_H
#include <string>
#include <vector>
#include <map>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#include "urdf_model/pose.h"
#include "urdf_model/joint.h"
#include "urdf_model/link.h"
namespace urdf{
class VisualSensor
{
public:
enum {CAMERA, RAY} type;
virtual ~VisualSensor(void)
{
}
};
class Camera : public VisualSensor
{
public:
Camera() { this->clear(); };
unsigned int width, height;
/// format is optional: defaults to R8G8B8), but can be
/// (L8|R8G8B8|B8G8R8|BAYER_RGGB8|BAYER_BGGR8|BAYER_GBRG8|BAYER_GRBG8)
std::string format;
double hfov;
double near;
double far;
void clear()
{
hfov = 0;
width = 0;
height = 0;
format.clear();
near = 0;
far = 0;
};
};
class Ray : public VisualSensor
{
public:
Ray() { this->clear(); };
unsigned int horizontal_samples;
double horizontal_resolution;
double horizontal_min_angle;
double horizontal_max_angle;
unsigned int vertical_samples;
double vertical_resolution;
double vertical_min_angle;
double vertical_max_angle;
void clear()
{
// set defaults
horizontal_samples = 1;
horizontal_resolution = 1;
horizontal_min_angle = 0;
horizontal_max_angle = 0;
vertical_samples = 1;
vertical_resolution = 1;
vertical_min_angle = 0;
vertical_max_angle = 0;
};
};
class Sensor
{
public:
Sensor() { this->clear(); };
/// sensor name must be unique
std::string name;
/// update rate in Hz
double update_rate;
/// transform from parent frame to optical center
/// with z-forward and x-right, y-down
Pose origin;
/// sensor
boost::shared_ptr<VisualSensor> sensor;
/// Parent link element name. A pointer is stored in parent_link_.
std::string parent_link_name;
boost::shared_ptr<Link> getParent() const
{return parent_link_.lock();};
void setParent(boost::shared_ptr<Link> parent)
{ this->parent_link_ = parent; }
void clear()
{
this->name.clear();
this->sensor.reset();
this->parent_link_name.clear();
this->parent_link_.reset();
};
private:
boost::weak_ptr<Link> parent_link_;
};
}
#endif
3rdParty/rbdl/addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_world/include/urdf_world/world.h deleted 100644 → 0
View file @ 2e754261
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* 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 Willow Garage 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.
*********************************************************************/
/* Author: John Hsu */
/* encapsulates components in a world
see http://ros.org/wiki/usdf/XML/urdf_world and
for details
*/
/* example world XML
<world name="pr2_with_table">
<!-- include the models by including
either the complete urdf or
referencing the file name. -->
<model name="pr2">
...
</model>
<include filename="table.urdf" model_name="table_model"/>
<!-- models in the world -->
<entity model="pr2" name="prj">
<origin xyz="0 1 0" rpy="0 0 0"/>
<twist linear="0 0 0" angular="0 0 0"/>
</entity>
<entity model="pr2" name="prk">
<origin xyz="0 2 0" rpy="0 0 0"/>
<twist linear="0 0 0" angular="0 0 0"/>
</entity>
<entity model="table_model">
<origin xyz="0 3 0" rpy="0 0 0"/>
<twist linear="0 0 0" angular="0 0 0"/>
</entity>
</world>
*/
#ifndef USDF_STATE_H
#define USDF_STATE_H
#include <string>
#include <vector>
#include <map>
#include <tinyxml.h>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#include "urdf_model/model.h"
#include "urdf_model/pose.h"
#include "urdf_model/twist.h"
namespace urdf{
class Entity
{
public:
boost::shared_ptr<ModelInterface> model;
Pose origin;
Twist twist;
};
class World
{
public:
World() { this->clear(); };
/// world name must be unique
std::string name;
std::vector<Entity> models;
void initXml(TiXmlElement* config);
void clear()
{
this->name.clear();
};
};
}
#endif
3rdParty/rbdl/addons/urdfreader/urdfreader.cc deleted 100644 → 0
View file @ 2e754261
#include <rbdl/rbdl.h>
#include "urdfreader.h"
#include <assert.h>
#include <iostream>
#include <fstream>
#include <map>
#include <stack>
#ifdef RBDL_USE_ROS_URDF_LIBRARY
#include <urdf_model/model.h>
#include <urdf_parser/urdf_parser.h>
typedef boost::shared_ptr<urdf::Link> LinkPtr;
typedef const boost::shared_ptr<const urdf::Link> ConstLinkPtr;
typedef boost::shared_ptr<urdf::Joint> JointPtr;
typedef boost::shared_ptr<urdf::ModelInterface> ModelPtr;
#else
#include <urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h>
#include <urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h>
typedef my_shared_ptr<urdf::Link> LinkPtr;
typedef const my_shared_ptr<const urdf::Link> ConstLinkPtr;
typedef my_shared_ptr<urdf::Joint> JointPtr;
typedef my_shared_ptr<urdf::ModelInterface> ModelPtr;
#endif
using namespace std;
namespace RigidBodyDynamics {
namespace Addons {
using namespace Math;
typedef vector<LinkPtr> URDFLinkVector;
typedef vector<JointPtr> URDFJointVector;
typedef map<string, LinkPtr > URDFLinkMap;
typedef map<string, JointPtr > URDFJointMap;
bool construct_model (Model* rbdl_model, ModelPtr urdf_model, bool floating_base, bool verbose) {
LinkPtr urdf_root_link;
URDFLinkMap link_map;
link_map = urdf_model->links_;
URDFJointMap joint_map;
joint_map = urdf_model->joints_;
vector<string> joint_names;
// Holds the links that we are processing in our depth first traversal with the top element being the current link.
stack<LinkPtr > link_stack;
// Holds the child joint index of the current link
stack<int> joint_index_stack;
// add the bodies in a depth-first order of the model tree
link_stack.push (link_map[(urdf_model->getRoot()->name)]);
// add the root body
ConstLinkPtr& root = urdf_model->getRoot ();
Vector3d root_inertial_rpy;
Vector3d root_inertial_position;
Matrix3d root_inertial_inertia;
double root_inertial_mass;
if (root->inertial) {
root_inertial_mass = root->inertial->mass;
root_inertial_position.set (
root->inertial->origin.position.x,
root->inertial->origin.position.y,
root->inertial->origin.position.z);
root_inertial_inertia(0,0) = root->inertial->ixx;
root_inertial_inertia(0,1) = root->inertial->ixy;
root_inertial_inertia(0,2) = root->inertial->ixz;
root_inertial_inertia(1,0) = root->inertial->ixy;
root_inertial_inertia(1,1) = root->inertial->iyy;
root_inertial_inertia(1,2) = root->inertial->iyz;
root_inertial_inertia(2,0) = root->inertial->ixz;
root_inertial_inertia(2,1) = root->inertial->iyz;
root_inertial_inertia(2,2) = root->inertial->izz;
root->inertial->origin.rotation.getRPY (root_inertial_rpy[0], root_inertial_rpy[1], root_inertial_rpy[2]);
Body root_link = Body (root_inertial_mass,
root_inertial_position,
root_inertial_inertia);
Joint root_joint (JointTypeFixed);
if (floating_base) {
root_joint = JointTypeFloatingBase;
}
SpatialTransform root_joint_frame = SpatialTransform ();
if (verbose) {
cout << "+ Adding Root Body " << endl;
cout << " joint frame: " << root_joint_frame << endl;
if (floating_base) {
cout << " joint type : floating" << endl;
} else {
cout << " joint type : fixed" << endl;
}
cout << " body inertia: " << endl << root_link.mInertia << endl;
cout << " body mass : " << root_link.mMass << endl;
cout << " body name : " << root->name << endl;
}
rbdl_model->AppendBody(root_joint_frame,
root_joint,
root_link,
root->name);
}
// depth first traversal: push the first child onto our joint_index_stack
joint_index_stack.push(0);
while (link_stack.size() > 0) {
LinkPtr cur_link = link_stack.top();
unsigned int joint_idx = joint_index_stack.top();
// Add any child bodies and increment current joint index if we still have child joints to process.
if (joint_idx < cur_link->child_joints.size()) {
JointPtr cur_joint = cur_link->child_joints[joint_idx];
// increment joint index
joint_index_stack.pop();
joint_index_stack.push (joint_idx + 1);
link_stack.push (link_map[cur_joint->child_link_name]);
joint_index_stack.push(0);
if (verbose) {
for (unsigned int i = 1; i < joint_index_stack.size() - 1; i++) {
cout << " ";
}
cout << "joint '" << cur_joint->name << "' child link '" << link_stack.top()->name << "' type = " << cur_joint->type << endl;
}
joint_names.push_back(cur_joint->name);
} else {
link_stack.pop();
joint_index_stack.pop();
}
}
unsigned int j;
for (j = 0; j < joint_names.size(); j++) {
JointPtr urdf_joint = joint_map[joint_names[j]];
LinkPtr urdf_parent = link_map[urdf_joint->parent_link_name];
LinkPtr urdf_child = link_map[urdf_joint->child_link_name];
// determine where to add the current joint and child body
unsigned int rbdl_parent_id = 0;
if (urdf_parent->name != "base_link") {
rbdl_parent_id = rbdl_model->GetBodyId (urdf_parent->name.c_str());
}
if (rbdl_parent_id == std::numeric_limits<unsigned int>::max())
cerr << "Error while processing joint '" << urdf_joint->name
<< "': parent link '" << urdf_parent->name
<< "' could not be found." << endl;
//cout << "joint: " << urdf_joint->name << "\tparent = " << urdf_parent->name << " child = " << urdf_child->name << " parent_id = " << rbdl_parent_id << endl;
// create the joint
Joint rbdl_joint;
if (urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::CONTINUOUS) {
rbdl_joint = Joint (SpatialVector (urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z, 0., 0., 0.));
} else if (urdf_joint->type == urdf::Joint::PRISMATIC) {
rbdl_joint = Joint (SpatialVector (0., 0., 0., urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
} else if (urdf_joint->type == urdf::Joint::FIXED) {
rbdl_joint = Joint (JointTypeFixed);
} else if (urdf_joint->type == urdf::Joint::FLOATING) {
// todo: what order of DoF should be used?
rbdl_joint = Joint (
SpatialVector (0., 0., 0., 1., 0., 0.),
SpatialVector (0., 0., 0., 0., 1., 0.),
SpatialVector (0., 0., 0., 0., 0., 1.),
SpatialVector (1., 0., 0., 0., 0., 0.),
SpatialVector (0., 1., 0., 0., 0., 0.),
SpatialVector (0., 0., 1., 0., 0., 0.));
} else if (urdf_joint->type == urdf::Joint::PLANAR) {
// todo: which two directions should be used that are perpendicular
// to the specified axis?
cerr << "Error while processing joint '" << urdf_joint->name << "': planar joints not yet supported!" << endl;
return false;
}
// compute the joint transformation
Vector3d joint_rpy;
Vector3d joint_translation;
urdf_joint->parent_to_joint_origin_transform.rotation.getRPY (joint_rpy[0], joint_rpy[1], joint_rpy[2]);
joint_translation.set (
urdf_joint->parent_to_joint_origin_transform.position.x,
urdf_joint->parent_to_joint_origin_transform.position.y,
urdf_joint->parent_to_joint_origin_transform.position.z
);
SpatialTransform rbdl_joint_frame =
Xrot (joint_rpy[0], Vector3d (1., 0., 0.))
* Xrot (joint_rpy[1], Vector3d (0., 1., 0.))
* Xrot (joint_rpy[2], Vector3d (0., 0., 1.))
* Xtrans (Vector3d (
joint_translation
));
// assemble the body
Vector3d link_inertial_position;
Vector3d link_inertial_rpy;
Matrix3d link_inertial_inertia = Matrix3d::Zero();
double link_inertial_mass = 0.;
// but only if we actually have inertial data
if (urdf_child->inertial) {
link_inertial_mass = urdf_child->inertial->mass;
link_inertial_position.set (
urdf_child->inertial->origin.position.x,
urdf_child->inertial->origin.position.y,
urdf_child->inertial->origin.position.z
);
urdf_child->inertial->origin.rotation.getRPY (link_inertial_rpy[0], link_inertial_rpy[1], link_inertial_rpy[2]);
link_inertial_inertia(0,0) = urdf_child->inertial->ixx;
link_inertial_inertia(0,1) = urdf_child->inertial->ixy;
link_inertial_inertia(0,2) = urdf_child->inertial->ixz;
link_inertial_inertia(1,0) = urdf_child->inertial->ixy;
link_inertial_inertia(1,1) = urdf_child->inertial->iyy;
link_inertial_inertia(1,2) = urdf_child->inertial->iyz;
link_inertial_inertia(2,0) = urdf_child->inertial->ixz;
link_inertial_inertia(2,1) = urdf_child->inertial->iyz;
link_inertial_inertia(2,2) = urdf_child->inertial->izz;
if (link_inertial_rpy != Vector3d (0., 0., 0.)) {
cerr << "Error while processing body '" << urdf_child->name << "': rotation of body frames not yet supported. Please rotate the joint frame instead." << endl;
return false;
}
}
Body rbdl_body = Body (link_inertial_mass, link_inertial_position, link_inertial_inertia);
if (verbose) {
cout << "+ Adding Body: " << urdf_child->name << endl;
cout << " parent_id : " << rbdl_parent_id << endl;
cout << " joint frame: " << rbdl_joint_frame << endl;
cout << " joint dofs : " << rbdl_joint.mDoFCount << endl;
for (unsigned int j = 0; j < rbdl_joint.mDoFCount; j++) {
cout << " " << j << ": " << rbdl_joint.mJointAxes[j].transpose() << endl;
}
cout << " body inertia: " << endl << rbdl_body.mInertia << endl;
cout << " body mass : " << rbdl_body.mMass << endl;
cout << " body name : " << urdf_child->name << endl;
}
if (urdf_joint->type == urdf::Joint::FLOATING) {
Matrix3d zero_matrix = Matrix3d::Zero();
Body null_body (0., Vector3d::Zero(3), zero_matrix);
Joint joint_txtytz(JointTypeTranslationXYZ);
string trans_body_name = urdf_child->name + "_Translate";
rbdl_model->AddBody (rbdl_parent_id, rbdl_joint_frame, joint_txtytz, null_body, trans_body_name);
Joint joint_euler_zyx (JointTypeEulerXYZ);
rbdl_model->AppendBody (SpatialTransform(), joint_euler_zyx, rbdl_body, urdf_child->name);
} else {
rbdl_model->AddBody (rbdl_parent_id, rbdl_joint_frame, rbdl_joint, rbdl_body, urdf_child->name);
}
}
return true;
}
RBDL_DLLAPI bool URDFReadFromFile (const char* filename, Model* model, bool floating_base, bool verbose) {
ifstream model_file (filename);
if (!model_file) {
cerr << "Error opening file '" << filename << "'." << endl;
abort();
}
// reserve memory for the contents of the file
string model_xml_string;
model_file.seekg(0, std::ios::end);
model_xml_string.reserve(model_file.tellg());
model_file.seekg(0, std::ios::beg);
model_xml_string.assign((std::istreambuf_iterator<char>(model_file)), std::istreambuf_iterator<char>());
model_file.close();
return URDFReadFromString (model_xml_string.c_str(), model, floating_base, verbose);
}
RBDL_DLLAPI bool URDFReadFromString (const char* model_xml_string, Model* model, bool floating_base, bool verbose) {
assert (model);
ModelPtr urdf_model = urdf::parseURDF (model_xml_string);
if (!construct_model (model, urdf_model, floating_base, verbose)) {
cerr << "Error constructing model from urdf file." << endl;
return false;
}
model->gravity.set (0., 0., -9.81);
return true;
}
}
}
3rdParty/rbdl/addons/urdfreader/urdfreader.h deleted 100644 → 0
View file @ 2e754261
#ifndef RBDL_URDFREADER_H
#define RBDL_URDFREADER_H
#include <rbdl/rbdl_config.h>
namespace RigidBodyDynamics {
struct Model;
namespace Addons {
RBDL_DLLAPI bool URDFReadFromFile (const char* filename, Model* model, bool floating_base, bool verbose = false);
RBDL_DLLAPI bool URDFReadFromString (const char* model_xml_string, Model* model, bool floating_base, bool verbose = false);
}
}
/* _RBDL_URDFREADER_H */
#endif