Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[10838] pkg/trunk
|
From: <is...@us...> - 2009-02-09 19:31:41
|
Revision: 10838
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10838&view=rev
Author: isucan
Date: 2009-02-09 19:31:32 +0000 (Mon, 09 Feb 2009)
Log Message:
-----------
copied the URDF parser part of wg_robot_description_parser to planning_models; large chunks of the parsing code will be removed, since we only need the data for planning (kinematic structure + dynamic properties)
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
pkg/trunk/util/self_watch/manifest.xml
pkg/trunk/util/self_watch/self_watch.cpp
pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt
pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
pkg/trunk/world_models/robot_models/planning_models/manifest.xml
pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
Added Paths:
-----------
pkg/trunk/world_models/robot_models/planning_models/include/planning_models/urdf.h
pkg/trunk/world_models/robot_models/planning_models/src/urdf.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-02-09 19:16:05 UTC (rev 10837)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-02-09 19:31:32 UTC (rev 10838)
@@ -185,13 +185,13 @@
return true;
}
- virtual void setRobotDescription(robot_desc::URDF *file)
+ virtual void setRobotDescription(planning_models::URDF *file)
{
KinematicStateMonitor::setRobotDescription(file);
if (m_kmodel)
{
std::vector<std::string> links;
- robot_desc::URDF::Group *g = file->getGroup("collision_check");
+ planning_models::URDF::Group *g = file->getGroup("collision_check");
if (g && g->hasFlag("collision"))
links = g->linkNames;
m_collisionSpace->lock();
@@ -218,9 +218,9 @@
protected:
- void addSelfCollisionGroups(unsigned int cid, robot_desc::URDF *model)
+ void addSelfCollisionGroups(unsigned int cid, planning_models::URDF *model)
{
- std::vector<robot_desc::URDF::Group*> groups;
+ std::vector<planning_models::URDF::Group*> groups;
model->getGroups(groups);
m_collisionSpace->lock();
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-02-09 19:16:05 UTC (rev 10837)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-02-09 19:31:32 UTC (rev 10838)
@@ -43,7 +43,7 @@
#include <ros/time.h>
#include <ros/console.h>
-#include <urdf/URDF.h>
+#include <planning_models/urdf.h>
#include <planning_models/kinematic.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
@@ -130,7 +130,7 @@
void setRobotDescriptionFromData(const char *data)
{
- robot_desc::URDF *file = new robot_desc::URDF();
+ planning_models::URDF *file = new planning_models::URDF();
if (file->loadString(data))
setRobotDescription(file);
else
@@ -139,14 +139,14 @@
void setRobotDescriptionFromFile(const char *filename)
{
- robot_desc::URDF *file = new robot_desc::URDF();
+ planning_models::URDF *file = new planning_models::URDF();
if (file->loadFile(filename))
setRobotDescription(file);
else
delete file;
}
- virtual void setRobotDescription(robot_desc::URDF *file)
+ virtual void setRobotDescription(planning_models::URDF *file)
{
if (m_urdf)
delete m_urdf;
@@ -296,7 +296,7 @@
ros::Node *m_node;
tf::TransformListener m_tf;
- robot_desc::URDF *m_urdf;
+ planning_models::URDF *m_urdf;
planning_models::KinematicModel *m_kmodel;
// info about the pose; this is not placed in the robot's kinematic state
Modified: pkg/trunk/motion_planning/kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-02-09 19:16:05 UTC (rev 10837)
+++ pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-02-09 19:31:32 UTC (rev 10838)
@@ -13,7 +13,6 @@
<depend package="robot_msgs" />
<depend package="robot_srvs" />
- <depend package="wg_robot_description_parser"/>
<depend package="planning_models"/>
<depend package="collision_space"/>
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-02-09 19:16:05 UTC (rev 10837)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-02-09 19:31:32 UTC (rev 10838)
@@ -389,7 +389,7 @@
return result;
}
- virtual void setRobotDescription(robot_desc::URDF *file)
+ virtual void setRobotDescription(planning_models::URDF *file)
{
CollisionSpaceMonitor::setRobotDescription(file);
defaultPosition();
@@ -688,12 +688,12 @@
void createMotionPlanningInstances(RKPModel* model)
{
std::map<std::string, std::string> options;
- robot_desc::URDF::Group *group = m_urdf->getGroup(model->kmodel->getURDFGroup(model->groupName));
+ planning_models::URDF::Group *group = m_urdf->getGroup(model->kmodel->getURDFGroup(model->groupName));
options.clear();
if (group)
{
- const robot_desc::URDF::Map &data = group->data;
+ const planning_models::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "RRT");
}
@@ -703,7 +703,7 @@
options.clear();
if (group)
{
- const robot_desc::URDF::Map &data = group->data;
+ const planning_models::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "LazyRRT");
}
model->addLazyRRT(options);
@@ -711,7 +711,7 @@
options.clear();
if (group)
{
- const robot_desc::URDF::Map &data = group->data;
+ const planning_models::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "EST");
}
model->addEST(options);
@@ -719,7 +719,7 @@
options.clear();
if (group)
{
- const robot_desc::URDF::Map &data = group->data;
+ const planning_models::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "SBL");
}
model->addSBL(options);
@@ -727,7 +727,7 @@
options.clear();
if (group)
{
- const robot_desc::URDF::Map &data = group->data;
+ const planning_models::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "IKSBL");
}
model->addIKSBL(options);
Modified: pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-02-09 19:16:05 UTC (rev 10837)
+++ pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-02-09 19:31:32 UTC (rev 10838)
@@ -200,7 +200,7 @@
}
}
- virtual void setRobotDescription(robot_desc::URDF *file)
+ virtual void setRobotDescription(planning_models::URDF *file)
{
CollisionSpaceMonitor::setRobotDescription(file);
Modified: pkg/trunk/util/self_watch/manifest.xml
===================================================================
--- pkg/trunk/util/self_watch/manifest.xml 2009-02-09 19:16:05 UTC (rev 10837)
+++ pkg/trunk/util/self_watch/manifest.xml 2009-02-09 19:31:32 UTC (rev 10838)
@@ -7,7 +7,7 @@
<depend package="rosconsole" />
<depend package="robot_msgs" />
- <depend package="wg_robot_description_parser" />
+
<depend package="planning_models" />
<depend package="collision_space" />
Modified: pkg/trunk/util/self_watch/self_watch.cpp
===================================================================
--- pkg/trunk/util/self_watch/self_watch.cpp 2009-02-09 19:16:05 UTC (rev 10837)
+++ pkg/trunk/util/self_watch/self_watch.cpp 2009-02-09 19:31:32 UTC (rev 10838)
@@ -66,7 +66,7 @@
if (getParam("robot_description", content))
{
// parse the description
- robot_desc::URDF *file = new robot_desc::URDF();
+ planning_models::URDF *file = new planning_models::URDF();
if (file->loadString(content.c_str()))
{
// create a kinematic model out of the parsed description
@@ -97,7 +97,7 @@
// get the list of links that are enabled for collision checking
std::vector<std::string> links;
- robot_desc::URDF::Group *g = file->getGroup("collision_check");
+ planning_models::URDF::Group *g = file->getGroup("collision_check");
if (g && g->hasFlag("collision"))
links = g->linkNames;
@@ -117,7 +117,7 @@
// get the self collision groups and add them to the collision space
int nscgroups = 0;
- std::vector<robot_desc::URDF::Group*> groups;
+ std::vector<planning_models::URDF::Group*> groups;
file->getGroups(groups);
for (unsigned int i = 0 ; i < groups.size() ; ++i)
if (groups[i]->hasFlag("self_collision"))
Modified: pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt 2009-02-09 19:16:05 UTC (rev 10837)
+++ pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt 2009-02-09 19:31:32 UTC (rev 10838)
@@ -1,7 +1,7 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(planning_models)
-rospack_add_library(planning_models src/kinematic.cpp)
+rospack_add_library(planning_models src/urdf.cpp src/kinematic.cpp)
# Unit tests
rospack_add_gtest(test_kinematic test/test_kinematic.cpp)
Modified: pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-02-09 19:16:05 UTC (rev 10837)
+++ pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-02-09 19:31:32 UTC (rev 10838)
@@ -37,7 +37,7 @@
#ifndef KINEMATIC_ROBOT_MODEL_
#define KINEMATIC_ROBOT_MODEL_
-#include <urdf/URDF.h>
+#include <planning_models/urdf.h>
#include <LinearMath/btTransform.h>
#include <iostream>
@@ -183,7 +183,7 @@
virtual void updateVariableTransform(const double *params) = 0;
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot) = 0;
+ virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot) = 0;
};
/** A fixed joint */
@@ -195,7 +195,7 @@
virtual void updateVariableTransform(const double *params);
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
+ virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot);
};
/** A planar joint */
@@ -212,7 +212,7 @@
virtual void updateVariableTransform(const double *params);
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
+ virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot);
};
/** A floating joint */
@@ -229,7 +229,7 @@
virtual void updateVariableTransform(const double *params);
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
+ virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot);
};
/** A prismatic joint */
@@ -247,7 +247,7 @@
virtual void updateVariableTransform(const double *params);
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
+ virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot);
btVector3 axis;
double limit[2];
@@ -268,7 +268,7 @@
virtual void updateVariableTransform(const double *params);
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
+ virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot);
btVector3 axis;
btVector3 anchor;
@@ -374,7 +374,7 @@
const double* computeTransform(const double *params, int groupID = -1);
/** Extract the information needed by the joint given the URDF description */
- void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
+ void extractInformation(const URDF::Link *urdfLink, Robot *robot);
};
@@ -558,7 +558,7 @@
}
void build(const std::string &description, bool ignoreSensors = false);
- virtual void build(const robot_desc::URDF &model, bool ignoreSensors = false);
+ virtual void build(const URDF &model, bool ignoreSensors = false);
bool isBuilt(void) const;
StateParams* newStateParams(void);
@@ -617,19 +617,19 @@
private:
/** Build the needed datastructure for a joint */
- void buildChainJ(Robot *robot, Link *parent, Joint *joint, const robot_desc::URDF::Link *urdfLink, const robot_desc::URDF &model);
+ void buildChainJ(Robot *robot, Link *parent, Joint *joint, const URDF::Link *urdfLink, const URDF &model);
/** Build the needed datastructure for a link */
- void buildChainL(Robot *robot, Joint *parent, Link *link, const robot_desc::URDF::Link *urdfLink, const robot_desc::URDF &model);
+ void buildChainL(Robot *robot, Joint *parent, Link *link, const URDF::Link *urdfLink, const URDF &model);
/** Construct the list of groups the model knows about (the ones marked with the 'plan' attribute) */
- void constructGroupList(const robot_desc::URDF &model);
+ void constructGroupList(const URDF &model);
/* compute the parameter names */
void computeParameterNames(void);
/** Allocate a joint of appropriate type, depending on the loaded link */
- Joint* createJoint(const robot_desc::URDF::Link* urdfLink);
+ Joint* createJoint(const URDF::Link* urdfLink);
};
Added: pkg/trunk/world_models/robot_models/planning_models/include/planning_models/urdf.h
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/include/planning_models/urdf.h (rev 0)
+++ pkg/trunk/world_models/robot_models/planning_models/include/planning_models/urdf.h 2009-02-09 19:31:32 UTC (rev 10838)
@@ -0,0 +1,851 @@
+/*********************************************************************
+ * 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 Ioan Sucan */
+
+#ifndef URDF_PARSER_
+#define URDF_PARSER_
+
+#include <tinyxml/tinyxml.h>
+#include <iostream>
+#include <string>
+#include <vector>
+#include <map>
+
+/**
+ @mainpage
+
+ @htmlinclude ../../manifest.html
+
+ Universal Robot Description Format (URDF) parser. The URDF class
+ is able to load a robot description from multiple files, string
+ buffers, streams and produce a datastructure similar to the XML
+ structure. Expression evaluation and file inclusion are performed
+ if needed. Checks are performed to make sure the document is
+ correct: values that are inconsistend are reported, typos in the
+ names, unknown tags, etc. For more documentation, see the URDF
+ description. */
+
+/** Namespace that contains the URDF parser */
+namespace planning_models
+{
+ /** This class contains a complete parser for URDF documents.
+ Datastructures that resemble the ones specified in the XML
+ document are instantiated, easy-access pointers are computed
+ (to access a datastructure from another). Checks are made to
+ make sure the parsed data is correct. Since actuators,
+ transmissions and controllers are potentially robot specific,
+ their data is kept as XML elements.
+ */
+ class URDF
+ {
+ public:
+
+ /** This class encapsulates data that can be attached to various tags in the format */
+ class Map
+ {
+ public:
+
+ Map(void)
+ {
+ }
+
+ virtual ~Map(void)
+ {
+ }
+
+ /** Get the set of flags used for the processed <map> tags. Each <map> tag has one flag. Multiple tags may
+ have the same flag. */
+ void getMapTagFlags(std::vector<std::string> &flags) const;
+
+ /** Given a specific flag, retrieve the names of the <map> tags that have the given flag */
+ void getMapTagNames(const std::string &flag, std::vector<std::string> &names) const;
+
+ /** Given a name and a flag, retrieve the defined map (string, string) */
+ std::map<std::string, std::string> getMapTagValues(const std::string &flag, const std::string &name) const;
+
+ /** Given a name and a flag, retrieve the defined map (string, XML) */
+ std::map<std::string, const TiXmlElement*> getMapTagXML(const std::string &flag, const std::string &name) const;
+
+ /** Check whether a key under empty flag and empty name has been defined */
+ bool hasDefault(const std::string &key) const;
+
+ /** Retrieve the value of a key (string) under empty flag and empty name has been defined */
+ std::string getDefaultValue(const std::string &key) const;
+
+ /** Retrieve the value of a key (XML) under empty flag and empty name has been defined */
+ const TiXmlElement* getDefaultXML(const std::string &key) const;
+
+ virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
+
+ /** Add a string element to the map */
+ void add(const std::string &flag, const std::string &name, const std::string &key, const std::string &value);
+
+ /** Add an XML element to the map */
+ void add(const std::string &flag, const std::string &name, const std::string &key, const TiXmlElement *value);
+
+ Map& operator=(const Map &rhs)
+ {
+ m_data = rhs.m_data;
+ return *this;
+ }
+ protected:
+
+ struct Element
+ {
+ Element(void)
+ {
+ xml = NULL;
+ str = NULL;
+ }
+
+ ~Element(void)
+ {
+ if (str)
+ delete str;
+ }
+
+ std::string *str; // allocated locally
+ const TiXmlElement *xml; // allocated in the XML document
+ };
+
+ std::map < std::string, std::map < std::string, std::map < std::string, Element > > > m_data;
+ };
+
+
+ /** Forward declaration of groups */
+ struct Group;
+
+ /** This class defines link instances. It contains instances of collision, visual, inertial and joint descriptions. */
+ struct Link
+ {
+
+ /** Class for link geometry instances */
+ struct Geometry
+ {
+ struct Shape
+ {
+ virtual ~Shape(void)
+ {
+ }
+ };
+
+ struct Sphere : public Shape
+ {
+ Sphere(void) : Shape()
+ {
+ radius = 0.0;
+ }
+
+ double radius;
+ };
+
+ struct Box : public Shape
+ {
+ Box(void) : Shape()
+ {
+ size[0] = size[1] = size[2] = 0.0;
+ }
+
+ double size[3];
+ };
+
+ struct Cylinder : public Shape
+ {
+ Cylinder(void) : Shape()
+ {
+ length = radius = 0.0;
+ }
+
+ double length, radius;
+ };
+
+ struct Mesh : public Shape
+ {
+ Mesh(void) : Shape()
+ {
+ scale[0] = scale[1] = scale[2] = 1.0;
+ }
+
+ std::string filename;
+ double scale[3];
+ };
+
+ Geometry(void)
+ {
+ type = UNKNOWN;
+ shape = NULL;
+ isSet["name"] = false;
+ isSet["size"] = false;
+ isSet["length"] = false;
+ isSet["radius"] = false;
+ isSet["filename"] = false;
+ isSet["scale"] = false;
+ }
+
+ virtual ~Geometry(void)
+ {
+ if (shape)
+ delete shape;
+ }
+
+ virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
+
+ enum
+ {
+ UNKNOWN, SPHERE, BOX, CYLINDER, MESH
+ } type;
+ std::string name;
+ Shape *shape;
+ Map data;
+ std::map<std::string, bool> isSet;
+ };
+
+
+ /** Class for link joint instances (connects a link to its parent) */
+ struct Joint
+ {
+ Joint(void)
+ {
+ axis[0] = axis[1] = axis[2] = 0.0;
+ anchor[0] = anchor[1] = anchor[2] = 0.0;
+ damping = 0.0;
+ friction = 0.0;
+ limit[0] = limit[1] = 0.0;
+ safetyLength[0] = safetyLength[1] = 0.0;
+ velocityLimit = 0.0;
+ effortLimit = 0.0;
+ type = UNKNOWN;
+ pjointMimic = NULL;
+ fMimicMult = 1.0; fMimicOffset = 0.0;
+ isSet["name"] = false;
+ isSet["type"] = false;
+ isSet["axis"] = false;
+ isSet["anchor"] = false;
+ isSet["damping"] = false;
+ isSet["friction"] = false;
+ isSet["limit"] = false;
+ isSet["safetyLengthMin"] = false;
+ isSet["safetyLengthMax"] = false;
+ isSet["effortLimit"] = false;
+ isSet["pjointMimic"] = false;
+ isSet["fMimicMult"] = false;
+ isSet["fMimicOffset"] = false;
+ isSet["velocityLimit"] = false;
+ isSet["calibration"] = false;
+ }
+
+ virtual ~Joint(void)
+ {
+ }
+
+ virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
+
+ enum
+ {
+ UNKNOWN, FIXED, REVOLUTE, PRISMATIC, PLANAR, FLOATING
+ } type;
+ std::string name;
+ double axis[3]; // vector describing the axis of rotation: (x,y,z)
+ double anchor[3]; // point about which the axis defines the rotation: (x,y,z)
+ double damping; // damping coefficient
+ double friction; // friction coefficient
+ double limit[2]; // the joint limits: (min, max)
+ double safetyLength[2]; // the joint limits: (min, max)
+ double effortLimit;
+ double velocityLimit;
+ std::string calibration;
+ Map data;
+ std::map<std::string, bool> isSet;
+
+ // only valid for joints that mimic other joint's values
+ Joint* pjointMimic; // if not NULL, this joint mimics pjointMimic
+ double fMimicMult, fMimicOffset; // the multiplication and offset coeffs. Ie, X = mult*Y+offset
+ // where Y is pjointMimic's joint value, X is this joint's value.
+ };
+
+ /** Class for link collision component instances */
+ struct Collision
+ {
+ Collision(void)
+ {
+ xyz[0] = xyz[1] = xyz[2] = 0.0;
+ rpy[0] = rpy[1] = rpy[2] = 0.0;
+ verbose = false;
+ geometry = NULL;
+ isSet["name"] = false;
+ isSet["verbose"] = false;
+ isSet["xyz"] = false;
+ isSet["rpy"] = false;
+ isSet["geometry"] = false;
+ }
+
+ virtual ~Collision(void)
+ {
+ if (geometry)
+ delete geometry;
+ }
+
+ virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
+
+ std::string name;
+ bool verbose;
+ double xyz[3];
+ double rpy[3];
+ Geometry *geometry;
+ Map data;
+ std::map<std::string, bool> isSet;
+ };
+
+ /** Class for link inertial component instances */
+ struct Inertial
+ {
+ Inertial(void)
+ {
+ mass = 0.0;
+ com[0] = com[1] = com[2] = 0.0;
+ inertia[0] = inertia[1] = inertia[2] = inertia[3] = inertia[4] = inertia[5] = 0.0;
+ isSet["name"] = false;
+ isSet["mass"] = false;
+ isSet["inertia"] = false;
+ isSet["com"] = false;
+ }
+
+ virtual ~Inertial(void)
+ {
+ }
+
+ virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
+
+ std::string name;
+ double mass;
+ /** Ixx Ixy Ixz Iyy Iyz Izz */
+ double inertia[6];
+ double com[3];
+ Map data;
+ std::map<std::string, bool> isSet;
+ };
+
+ /** Class for link visual component instances */
+ struct Visual
+ {
+ Visual(void)
+ {
+ xyz[0] = xyz[1] = xyz[2] = 0.0;
+ rpy[0] = rpy[1] = rpy[2] = 0.0;
+ geometry = NULL;
+ isSet["name"] = false;
+ isSet["xyz"] = false;
+ isSet["rpy"] = false;
+ isSet["geometry"] = false;
+ }
+
+ virtual ~Visual(void)
+ {
+ if (geometry)
+ delete geometry;
+ }
+
+ virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
+
+ std::string name;
+ double xyz[3];
+ double rpy[3];
+ Geometry *geometry;
+ Map data;
+ std::map<std::string, bool> isSet;
+ };
+
+ Link(void)
+ {
+ parent = NULL;
+ xyz[0] = xyz[1] = xyz[2] = 0.0;
+ rpy[0] = rpy[1] = rpy[2] = 0.0;
+ inertial = NULL;
+ visual = NULL;
+ collision = NULL;
+ joint = NULL;
+ isSet["name"] = false;
+ isSet["parent"] = false;
+ isSet["inertial"] = false;
+ isSet["visual"] = false;
+ isSet["collision"] = false;
+ isSet["joint"] = false;
+ isSet["xyz"] = false;
+ isSet["rpy"] = false;
+ }
+
+ virtual ~Link(void)
+ {
+ if (inertial)
+ delete inertial;
+ if (visual)
+ delete visual;
+ if (collision)
+ delete collision;
+ if (joint)
+ delete joint;
+ }
+
+ /** Check if the link instance is inside some group */
+ bool insideGroup(Group *group) const;
+
+ /** Check if the link instance is inside some group */
+ bool insideGroup(const std::string &group) const;
+
+ /** Check if the link instance is also a sensor */
+ virtual bool canSense(void) const;
+
+ virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
+
+ Link *parent;
+ std::string parentName;
+ std::string name;
+ std::vector<Link*> children;
+
+ Inertial *inertial;
+ Visual *visual;
+ Collision *collision;
+ Joint *joint;
+
+ double rpy[3];
+ double xyz[3];
+ Map data;
+
+ std::vector<Group*> groups;
+ std::vector<bool> inGroup;
+
+ std::map<std::string, bool> isSet;
+ };
+
+ /** Class for defining sensors. This is basically a link with a few extra parameters */
+ struct Sensor : public Link
+ {
+ Sensor(void)
+ {
+ type = UNKNOWN;
+ isSet["type"] = false;
+ isSet["calibration"] = false;
+ }
+
+ virtual ~Sensor(void)
+ {
+ }
+
+ virtual bool canSense(void) const;
+ virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
+
+ enum
+ {
+ UNKNOWN, LASER, CAMERA, STEREO_CAMERA
+ } type;
+ std::string calibration;
+ };
+
+ /** Class for defining frames. A frame is attached to a link,
+ as its parent or as its child. Frames do not have
+ geometric or inertial properties. They are used to define
+ coordinate frames. */
+ struct Frame
+ {
+ Frame(void)
+ {
+ link = NULL;
+ type = UNKNOWN;
+ xyz[0] = xyz[1] = xyz[2] = 0.0;
+ rpy[0] = rpy[1] = rpy[2] = 0.0;
+ isSet["name"] = false;
+ isSet["parent"] = false;
+ isSet["child"] = false;
+ isSet["rpy"] = false;
+ isSet["xyz"] = false;
+ }
+
+ virtual ~Frame(void)
+ {
+ }
+
+ virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
+
+ std::string name;
+ std::string linkName;
+ Link* link;
+
+ enum
+ {
+ UNKNOWN, PARENT, CHILD
+ } type;
+
+ double rpy[3];
+ double xyz[3];
+ Map data;
+
+ std::vector<Group*> groups;
+ std::map<std::string, bool> isSet;
+ };
+
+ /** A class that represents a link chain */
+ struct Chain
+ {
+ Chain(void)
+ {
+ }
+
+ virtual ~Chain(void)
+ {
+ }
+
+ virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
+
+ std::string name;
+ std::string root;
+ std::string tip;
+ std::vector<Link*> links;
+ };
+
+ /** A class that represents groups of links or frames (or both) */
+ struct Group
+ {
+ Group(void)
+ {
+ }
+
+ virtual ~Group(void)
+ {
+ }
+
+ /** Check if this group has a specific flag */
+ bool hasFlag(const std::string &flag) const;
+
+ /** Check if a specific link is a root in this group */
+ bool isRoot(const Link* link) const;
+
+ /** Check if the group is empty */
+ bool empty(void) const;
+
+ virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
+
+ std::string name;
+ std::vector<std::string> flags;
+ std::vector<std::string> linkNames;
+ std::vector<Link*> links;
+ std::vector<Link*> linkRoots;
+ std::vector<std::string> frameNames;
+ std::vector<Frame*> frames;
+ Map data;
+ };
+
+ /** Constructor. If a filename if specified as argument, that file is parsed. */
+ URDF(const char *filename = NULL)
+ {
+ m_paths.push_back("");
+ m_errorCount = 0;
+ m_rememberUnknownTags = false;
+ m_verbose = true;
+ if (filename)
+ loadFile(filename);
+ }
+
+ /** Destructor. Frees all memory */
+ virtual ~URDF(void)
+ {
+ freeMemory();
+ }
+
+ /** Clear all datastructures. The parser can be used for a new document */
+ virtual void clear(void);
+
+ /** Parse the content of the specified file */
+ virtual bool loadFile(const char *filename);
+
+ /** Parse the content of the specified file */
+ virtual bool loadFile(FILE *file);
+
+ /** Parse the content of the specified buffer */
+ virtual bool loadString(const char *data);
+
+ /** Parse the content of the specified stream */
+ virtual bool loadStream(std::istream &is);
+
+ /** Print the parsed datastructure */
+ virtual void print(std::ostream &out = std::cout) const;
+
+ /** Set the verbosity. Default is true */
+ void setVerbose(bool verbose);
+
+ /** If set to true, unknown tags are stored in a list instead
+ of presenting error messages. Default is false. */
+ void rememberUnknownTags(bool remember);
+
+ /** Returns the robot name */
+ const std::string& getRobotName(void) const;
+
+ /** Returns the number of individual robot parts that connect to the environment */
+ unsigned int getDisjointPartCount(void) const;
+
+ /** Return a robot part that connects to the environment */
+ Link* getDisjointPart(unsigned int index) const;
+
+ /** Check if a specific link connects to the environment */
+ bool isRoot(const Link* link) const;
+
+ /** Retrieve a link by its name */
+ Link* getLink(const std::string &name) const;
+
+ /** Retrieve a link by its joint name (connecting to the link's parent) */
+ Link* getJointLink(const std::string &name) const;
+
+ /** Get the list of all links. The array is sorted alphabetically by name. */
+ void getLinks(std::vector<Link*> &links) const;
+
+ /** Retrieve a frame by its name */
+ Frame* getFrame(const std::string &name) const;
+
+ /** Get the list of all frames. The array is sorted alphabetically by name. */
+ void getFrames(std::vector<Frame*> &frames) const;
+
+ /** Get the list of all group names. The array is sorted alphabetically. */
+ void getGroupNames(std::vector<std::string> &groups) const;
+
+ /** Retrieve a group by its name */
+ Group* getGroup(const std::string &name) const;
+
+ /** Get the list of all groups. The array is sorted alphabetically by name. */
+ void getGroups(std::vector<Group*> &groups) const;
+
+ /** Retrieve a chain by its name */
+ Chain* getChain(const std::string &name) const;
+
+ /** Get the list of all chains. The array is sorted alphabetically by name. */
+ void getChains(std::vector<Chain*> &chains) const;
+
+ /** Get the data that was defined at top level */
+ const Map& getMap(void) const;
+
+ /** Get the defined resource path for this document */
+ std::string getResourceLocation(void) const;
+
+ /** Try to evaluate the constant as a double value */
+ double getConstantValue(const std::string &name, bool *error = NULL) const;
+
+ /** Try to retrieve the constant as a string value */
+ std::string getConstantString(const std::string &name, bool *error = NULL) const;
+
+ /** Get the list of actuator tags */
+ void getActuators(std::vector<const TiXmlElement*> &actuators) const;
+
+ /** Get the list of transmission tags */
+ void getTransmissions(std::vector<const TiXmlElement*> &transmissions) const;
+
+ /** Get the list of controller tags */
+ void getControllers(std::vector<const TiXmlElement*> &controllers) const;
+
+ /** Get the list of unknown tags. Only available if
+ rememberUnknownTags(true) was called before parsing. */
+ void getUnknownTags(std::vector<const TiXmlNode*> &unknownTags) const;
+
+ /** Check if the links form a cycle */
+ bool containsCycle(unsigned int index) const;
+
+ /** Simple checks to make sure the parsed values are correct */
+ void sanityCheck(void) const;
+
+ /** Return the number of encountered errors */
+ unsigned int getErrorCount(void) const;
+
+ protected:
+
+ /** Free the memory allocated in this class */
+ void freeMemory(void);
+
+ /** Clear temporary datastructures used in parsing */
+ void clearTemporaryData(void);
+
+ /** Clear the memory used by the XML parser*/
+ void clearDocs(void);
+
+
+
+ /** Add the path to a given filename as a known path */
+ virtual void addPath(const char *filename);
+
+ /** Find a file using the set of known paths */
+ virtual char* findFile(const char *filename);
+
+
+
+ /** Add default constants like M_PI */
+ virtual void defaultConstants(void);
+
+ /** Parse the URDF document */
+ virtual bool parse(const TiXmlNode *node);
+
+ /** Parse the <link> tag */
+ void loadLink(const TiXmlNode *node);
+ /** Parse the <sensor> tag */
+ void loadSensor(const TiXmlNode *node);
+ /** Parse the <frame> tag */
+ void loadFrame(const TiXmlNode *node);
+ /** Parse the <joint> tag */
+ void loadJoint(const TiXmlNode *node, const std::string &defaultName, Link::Joint *joint);
+ /** Parse the <geometry> tag */
+ void loadGeometry(const TiXmlNode *node, const std::string &defaultName, Link::Geometry *geometry);
+ /** Parse the <collision> tag */
+ void loadCollision(const TiXmlNode *node, const std::string &defaultName, Link::Collision *collision);
+ /** Parse the <visual> tag */
+ void loadVisual(const TiXmlNode *node, const std::string &defaultName, Link::Visual *visual);
+ /** Parse the <inertial> tag */
+ void loadInertial(const TiXmlNode *node, const std::string &defaultName, Link::Inertial *inertial);
+ /** Parse the <data> tag */
+ void loadMap(const TiXmlNode *node, Map *data);
+
+ /** Parse a list of string expressions and convert them to doubles (this relies on the loaded constants) */
+ unsigned int loadDoubleValues(const TiXmlNode *node, unsigned int count, double *vals, const char *attrName = NULL, bool warn = false);
+ /** Parse a list of string expressions and convert them to doubles (this relies on the loaded constants) */
+ unsigned int loadDoubleValues(const std::string& data, unsigned int count, double *vals, const TiXmlNode *node = NULL);
+ /** Parse a list of strings and convert them to booleans */
+ unsigned int loadBoolValues (const TiXmlNode *node, unsigned int count, bool *vals, const char *attrName = NULL, bool warn = false);
+ /** Parse a list of string expressions and convert them to doubles (this relies on the loaded constants) */
+ unsigned int loadBoolValues(const std::string& data, unsigned int count, bool *vals, const TiXmlNode *node = NULL);
+
+
+ /** Depending on whether the node is a clone or not, get the proper list of children and attibutes for this tag */
+ void getChildrenAndAttributes(const TiXmlNode* node, std::vector<const TiXmlNode*> &children, std::vector<const TiXmlAttribute*> &attributes) const;
+
+ /** Extract and return the value of the name attribute from a list of attributes */
+ std::string extractName(std::vector<const TiXmlAttribute*> &attributes, const std::string &defaultName) const;
+
+ /** Behaviour for unknown nodes. Default is to output a message letting the user know that a specific node is unknown */
+ virtual void unknownNode(const TiXmlNode* node);
+
+ /** Replace include declarations with the indicated file */
+ bool replaceIncludes(TiXmlElement *elem);
+
+ /** Print the error location, if known */
+ void errorLocation(const TiXmlNode* node = NULL) const;
+
+ /** Output an error message */
+ void errorMessage(const std::string& msg) const;
+
+ /** Compute the easy-access pointers inside the parsed datastructures */
+ virtual void linkDatastructure(void);
+
+ /** The name of the file where the parsing started */
+ std::string m_source;
+
+ /** A string that defines the base path for resources pointed to from the loaded document */
+ std::string m_resourceLocation;
+
+ /** The list of paths the parser knows about when it sees an <include> directive */
+ std::vector<std::string> m_paths;
+
+ /** The robot name */
+ std::string m_name;
+
+ /** Contains the list of parsed links and sensors */
+ std::map<std::string, Link*> m_links;
+
+ /** Contains the list of parsed groups */
+ std::map<std::string, Group*> m_groups;
+
+ /** Contains the list of parsed frames */
+ std::map<std::string, Frame*> m_frames;
+
+ /** The list of parsed chains */
+ std::map<std::string, Chain*> m_chains;
+
+ /** Contains information specified in <data> tags at the top level */
+ Map m_data;
+
+ /** Additional datastructure containing a list links that are connected to the environment */
+ std::vector<Link*> m_linkRoots;
+
+ /** The list of constants */
+ std::map<std::string, std::string> m_constants;
+
+ /** The list of constant blocks */
+ std::map<std::string, const TiXmlNode*> m_constBlocks;
+
+ /** The list of actuator tags */
+ std::vector<const TiXmlElement*> m_actuators;
+
+ /** The list of controller tags */
+ std::vector<const TiXmlElement*> m_controllers;
+
+ /** The list of transmission tags */
+ std::vector<const TiXmlElement*> m_transmissions;
+
+ /** If this is set to true, unknown tags will be simply added
+ to a list, no error messages related to unknown tags will
+ be presented. */
+ bool m_rememberUnknownTags;
+
+ /** The list of unknown tags (maintained only if rememberUnknownTags(true) was previosuly called) */
+ std::vector<const TiXmlNode*> m_unknownTags;
+
+ /** Verbosity flag */
+ bool m_verbose;
+
+ /** Counter for errors */
+ mutable unsigned int m_errorCount;
+
+ private:
+
+ /* temporary storage for information during parsing; should not be used elsewhere */
+
+ /** Temporary datastructure for keeping track link collision components */
+ std::map<std::string, Link::Collision*> m_collision;
+ /** Temporary datastructure for keeping track link joints */
+ std::map<std::string, Link::Joint*> m_joints;
+ /** Temporary datastructure for keeping track link inertial components */
+ std::map<std::string, Link::Inertial*> m_inertial;
+ /** Temporary datastructure for keeping track link visual components */
+ std::map<std::string, Link::Visual*> m_visual;
+ /** Temporary datastructure for keeping track link geometry */
+ std::map<std::string, Link::Geometry*> m_geoms;
+
+
+ /** List of nodes to be processed after loading the constants and templates (temporary) */
+ std::vector<const TiXmlNode*> m_stage2;
+ /** List of loaded documents. The documents are not cleared after parse since <data> tags may contain pointers to XML datastructures from these documents */
+ std::vector<TiXmlDocument*> m_docs;
+
+ /** Approximate location in parsed file, if known (used for error messages) */
+ std::string m_location;
+ };
+
+} // namespace robot_desc
+
+#endif
+
Modified: pkg/trunk/world_models/robot_models/planning_models/manifest.xml
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/manifest.xml 2009-02-09 19:16:05 UTC (rev 10837)
+++ pkg/trunk/world_models/robot_models/planning_models/manifest.xml 2009-02-09 19:31:32 UTC (rev 10838)
@@ -8,7 +8,8 @@
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/RobotDescriptionFormat</url>
- <depend package="wg_robot_description_parser"/>
+ <depend package="tinyxml"/>
+ <depend package="math_expr"/>
<depend package="bullet"/>
<export>
Modified: pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp 2009-02-09 19:16:05 UTC (rev 10837)
+++ pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp 2009-02-09 19:31:32 UTC (rev 10838)
@@ -146,7 +146,7 @@
// the joint remains identity
}
-void planning_models::KinematicModel::FixedJoint::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot)
+void planning_models::KinematicModel::FixedJoint::extractInformation(const URDF::Link *urdfLink, Robot *robot)
{
// we need no data
}
@@ -162,7 +162,7 @@
varTrans.setRotation(newQuat);
}
-void planning_models::KinematicModel::PlanarJoint::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot)
+void planning_models::KinematicModel::PlanarJoint::extractInformation(const URDF::Link *urdfLink, Robot *robot)
{
robot->stateBounds.insert(robot->stateBounds.end(), 4, 0.0);
robot->stateBounds.push_back(-M_PI);
@@ -176,7 +176,7 @@
varTrans.setRotation(btQuaternion(btScalar(params[3]), btScalar(params[4]), btScalar(params[5]), btScalar(params[6])));
}
-void planning_models::KinematicModel::FloatingJoint::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot)
+void planning_models::KinematicModel::FloatingJoint::extractInformation(const URDF::Link *urdfLink, Robot *robot)
{
robot->stateBounds.insert(robot->stateBounds.end(), 14, 0.0);
robot->floatingJoints.push_back(robot->stateDimension);
@@ -189,7 +189,7 @@
varTrans.setOrigin(newOrigin);
}
-void planning_models::KinematicModel::PrismaticJoint::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot)
+void planning_models::KinematicModel::PrismaticJoint::extractInformation(const URDF::Link *urdfLink, Robot *robot)
{
axis.setX(btScalar(urdfLink->joint->axis[0]));
axis.setY(btScalar(urdfLink->joint->axis[1]));
@@ -215,7 +215,7 @@
varTrans *= part3;
}
-void planning_models::KinematicModel::RevoluteJoint::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot)
+void planning_models::KinematicModel::RevoluteJoint::extractInformation(const URDF::Link *urdfLink, Robot *robot)
{
axis.setX(urdfLink->joint->axis[0]);
axis.setY(urdfLink->joint->axis[1]);
@@ -240,33 +240,33 @@
robot->stateBounds.push_back(limit[1]);
}
-void planning_models::KinematicModel::Link::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot)
+void planning_models::KinematicModel::Link::extractInformation(const URDF::Link *urdfLink, Robot *robot)
{
/* compute the geometry for this link */
switch (urdfLink->collision->geometry->type)
{
- case robot_desc::URDF::Link::Geometry::BOX:
+ case URDF::Link::Geometry::BOX:
{
Box *box = new Box();
- const double *size = static_cast<const robot_desc::URDF::Link::Geometry::Box*>(urdfLink->collision->geometry->shape)->size;
+ const double *size = static_cast<const URDF::Link::Geometry::Box*>(urdfLink->collision->geometry->shape)->size;
box->size[0] = size[0];
box->size[1] = size[1];
box->size[2] = size[2];
shape = box;
}
break;
- case robot_desc::URDF::Link::Geometry::SPHERE:
+ case URDF::Link::Geometry::SPHERE:
{
Sphere *sphere = new Sphere();
- sphere->radius = static_cast<const robot_desc::URDF::Link::Geometry::Sphere*>(urdfLink->collision->geometry->shape)->radius;
+ sphere->radius = static_cast<const URDF::Link::Geometry::Sphere*>(urdfLink->collision->geometry->shape)->radius;
shape = sphere;
}
break;
- case robot_desc::URDF::Link::Geometry::CYLINDER:
+ case URDF::Link::Geometry::CYLINDER:
{
Cylinder *cylinder = new Cylinder();
- cylinder->length = static_cast<const robot_desc::URDF::Link::Geometry::Cylinder*>(urdfLink->collision->geometry->shape)->length;
- cylinder->radius = static_cast<const robot_desc::URDF::Link::Geometry::Cylinder*>(urdfLink->collision->geometry->shape)->radius;
+ cylinder->length = static_cast<const URDF::Link::Geometry::Cylinder*>(urdfLink->collision->geometry->shape)->length;
+ cylinder->radius = static_cast<const URDF::Link::Geometry::Cylinder*>(urdfLink->collision->geometry->shape)->radius;
shape = cylinder;
}
break;
@@ -291,7 +291,7 @@
m_verbose = verbose;
}
-void planning_models::KinematicModel::constructGroupList(const robot_desc::URDF &model)
+void planning_models::KinematicModel::constructGroupList(const URDF &model)
{
std::string rname = model.getRobotName();
std::vector<std::string> allGroups;
@@ -358,13 +358,13 @@
void planning_models::KinematicModel::build(const std::string &description, bool ignoreSensors)
{
- robot_desc::URDF *file = new robot_desc::URDF();
+ URDF *file = new URDF();
file->loadString(description.c_str());
build(*file, ignoreSensors);
delete file;
}
-void planning_models::KinematicModel::build(const robot_desc::URDF &model, bool ignoreSensors)
+void planning_models::KinematicModel::build(const URDF &model, bool ignoreSensors)
{
if (m_built)
{
@@ -383,7 +383,7 @@
for (unsigned int i = 0 ; i < model.getDisjointPartCount() ; ++i)
{
- const robot_desc::URDF::Link *link = model.getDisjointPart(i);
+ const URDF::Link *link = model.getDisjointPart(i);
if (link->canSense() && m_ignoreSensors)
continue;
Robot *rb = new Robot(this);
@@ -503,7 +503,7 @@
names.push_back(joints[i]->name);
}
-void planning_models::KinematicModel::buildChainJ(Robot *robot, Link *parent, Joint* joint, const robot_desc::URDF::Link* urdfLink, const robot_desc::URDF &model)
+void planning_models::KinematicModel::buildChainJ(Robot *robot, Link *parent, Joint* joint, const URDF::Link* urdfLink, const URDF &model)
{
joint->before = parent;
joint->after = new Link();
@@ -546,7 +546,7 @@
buildChainL(robot, joint, joint->after, urdfLink, model);
}
-void planning_models::KinematicModel::buildChainL(Robot *robot, Joint *parent, Link* link, const robot_desc::URDF::Link* urdfLink, const robot_desc::URDF &model)
+void planning_models::KinematicModel::buildChainL(Robot *robot, Joint *parent, Link* link, const URDF::Link* urdfLink, const URDF &model)
{
link->name = urdfLink->name;
link->before = parent;
@@ -565,24 +565,24 @@
}
}
-planning_models::KinematicModel::Joint* planning_models::KinematicModel::createJoint(const robot_desc::URDF::Link* urdfLink)
+planning_models::KinematicModel::Joint* planning_models::KinematicModel::createJoint(const URDF::Link* urdfLink)
{
Joint *newJoint = NULL;
switch (urdfLink->joint->type)
{
- case robot_desc::URDF::Link::Joint::FIXED:
+ case URDF::Link::Joint::FIXED:
newJoint = new FixedJoint();
break;
- case robot_desc::URDF::Link::Joint::FLOATING:
+ case URDF::Link::Joint::FLOATING:
newJoint = new FloatingJoint();
break;
- case robot_desc::URDF::Link::Joint::PLANAR:
+ case URDF::Link::Joint::PLANAR:
newJoint = new PlanarJoint();
break;
- case robot_desc::URDF::Link::Joint::PRISMATIC:
+ case URDF::Link::Joint::PRISMATIC:
newJoint = new PrismaticJoint();
break;
- case robot_desc::URDF::Link::Joint::REVOLUTE:
+ case URDF::Link::Joint::REVOLUTE:
newJoint = new RevoluteJoint();
break;
default:
Added: pkg/trunk/world_models/robot_models/planning_models/src/urdf.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/src/urdf.cpp (rev 0)
+++ pkg/trunk/world_models/robot_models/planning_models/src/urdf.cpp 2009-02-09 19:31:32 UTC (rev 10838)
@@ -0,0 +1,2456 @@
+/*********************************************************************
+ * 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 Ioan Sucan */
+
+#include <planning_models/urdf.h>
+#include <math_expr/MathExpression.h>
+#include <boost/algorithm/string.hpp>
+#include <boost/lexical_cast.hpp>
+#include <algorithm>
+#include <cstring>
+#include <fstream>
+#include <sstream>
+#include <queue>
+
+
+namespace planning_models {
+
+ /* Macro to mark the fact a certain member variable was set. Also
+ prints a warning if the same member was set multiple times. */
+#define MARK_SET(node, owner, variable) \
+ { \
+ if (owner->isSet[#variable]) { \
+ errorMessage("'" + std::string(#variable) + \
+ "' already set"); \
+ errorLocation(node); } \
+ else \
+ owner->isSet[#variable] = true; \
+ }
+
+
+ /* Operator for sorting objects by name */
+ template<typename T>
+ struct SortByName
+ {
+ bool operator()(const T *a, const T *b) const
+ {
+ return a->name < b->name;
+ }
+ };
+
+ void URDF::freeMemory(void)
+ {
+ clearDocs();
+
+ for (std::map<std::string, Link*>::iterator i = m_links.begin() ; i != m_links.end() ; i++)
+ delete i->second;
+ for (std::map<std::string, Group*>::iterator i = m_groups.begin() ; i != m_groups.end() ; i++)
+ delete i->second;
+ for (std::map<std::string, Frame*>::iterator i = m_frames.begin() ; i != m_frames.end() ; i++)
+ delete i->second;
+ for (std::map<std::string, Chain*>::iterator i = m_chains.begin() ; i != m_chains.end() ; i++)
+ delete i->second;
+ }
+
+ void URDF::clear(void)
+ {
+ freeMemory();
+ m_name.clear();
+ m_source.clear();
+ m_resourceLocation.clear();
+ m_location.clear();
+ m_links.clear();
+ m_frames.clear();
+ m_groups.clear();
+ m_chains.clear();
+ m_paths.clear();
+ m_paths.push_back("");
+ m_linkRoots.clear();
+ clearTemporaryData();
+ m_errorCount = 0;
+ }
+
+ void URDF::setVerbose(bool verbose)
+ {
+ m_verbose = verbose;
+ }
+
+ void URDF::rememberUnknownTags(bool remember)
+ {
+ m_rememberUnknownTags = remember;
+ }
+
+ unsigned int URDF::getErrorCount(void) const
+ {
+ return m_errorCount;
+ }
+
+ const std::string& URDF::getRobotName(void) const
+ {
+ return m_name;
+ }
+
+ std::string URDF::getResourceLocation(void) const
+ {
+ return m_resourceLocation;
+ }
+
+ URDF::Link* URDF::getLink(const std::string &name) const
+ {
+ std::map<std::string, Link*>::const_iterator it = m_links.find(name);
+ return it == m_links.end() ? NULL : it->second;
+ }
+
+ URDF::Link* URDF::getJointLink(const std::string &name) const
+ {
+ for (std::map<std::string, Link*>::const_iterator i = m_links.begin() ; i != m_links.end() ; i++)
+ if (i->second->joint->name == name)
+ return i->second;
+ return NULL;
+ }
+
+ void URDF::getLinks(std::vector<Link*> &links) const
+ {
+ std::vector<Link*> localLinks;
+ for (std::map<std::string, Link*>::const_iterator i = m_links.begin() ; i != m_links.end() ; i++)
+ localLinks.push_back(i->second);
+ std::sort(localLinks.begin(), localLinks.end(), SortByName<Link>());
+ links.insert(links.end(), localLinks.begin(), localLinks.end());
+ }
+
+ URDF::Frame* URDF::getFrame(const std::string &name) const
+ {
+ std::map<std::string, Frame*>::const_iterator it = m_frames.find(name);
+ return it == m_frames.end() ? NULL : it->second;
+ }
+
+ void URDF::getFrames(std::vector<Frame*> &frames) const
+ {
+ std::vector<Frame*> localFrames;
+ for (std::map<std::string, Frame*>::const_iterator i = m_frames.begin() ; i != m_frames.end() ; i++)
+ localFrames.push_back(i->second);
+ std::sort(localFrames.begin(), localFrames.end(), SortByName<Frame>());
+ frames.insert(frames.end(), localFrames.begin(), localFrames.end());
+ }
+
+ unsigned int URDF::getDisjointPartCount(void) const
+ {
+ return m_linkRoots.size();
+ }
+
+ URDF::Link* URDF::getDisjointPart(unsigned int index) const
+ {
+ if (index < m_linkRoots.size())
+ return m_linkRoots[index];
+ else
+ return NULL;
+ }
+
+ bool URDF::isRoot(const Link* link) const
+ {
+ for (unsigned int i = 0 ; i < m_linkRoots.size() ; ++i)
+ if (link == m_linkRoots[i])
+ return true;
+ return false;
+ }
+
+ void URDF::getGroupNames(std::vector<std::string> &groups) const
+ {
+ std::vector<std::string> localGroups;
+ for (std::map<std::string, Group*>::const_iterator i = m_groups.begin() ; i != m_groups.end() ; i++)
+ localGroups.push_back(i->first);
+ std::sort(localGroups.begin(), localGroups.end());
+ groups.insert(groups.end(), localGroups.begin(), localGroups.end());
+ }
+
+ URDF::Group* URDF::getGroup(const std::string &name) const
+ {
+ std::map<std::string, Group*>::const_iterator it = m_groups.find(name);
+ return (it == m_groups.end()) ? NULL : it->second;
+ }
+
+ void URDF::getGroups(std::vector<Group*> &groups) const
+ {
+ /* To maintain the same ordering as getGroupNames, we do this in a slightly more cumbersome way */
+ std::vector<std::string> names;
+ getGroupNames(names);
+ for (unsigned int i = 0 ; i < names.size() ; ++i)
+ groups.push_back(getGroup(names[i]));
+ }
+
+ URDF::Chain* URDF::getChain(const std::string &name) const
+ {
+ std::map<std::string, Chain*>::const_iterator it = m_chains.find(name);
+ return (it == m_chains.end()) ? NULL : it->second;
+ }
+
+ void URDF::getChains(std::vector<Chain*> &chains) const
+ {
+ std::vector<Chain*> localChains;
+ for (std::map<std::string, Chain*>::const_iterator i = m_chains.begin() ; i != m_chains.end() ; i++)
+ localChains.push_back(i->second);
+ std::sort(localChains.begin(), localChains.end(), SortByName<Chain>());
+ chains.insert(chains.end(), localChains.begin(), localChains.end());
+ }
+
+ const URDF::Map& URDF::getMap(void) const
+ {
+ ...
[truncated message content] |
