|
From: <stu...@us...> - 2008-08-19 21:52:28
|
Revision: 3273
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3273&view=rev
Author: stuglaser
Date: 2008-08-19 21:52:35 +0000 (Tue, 19 Aug 2008)
Log Message:
-----------
Subversion choked, so now these changes are being added as one commit.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h
pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h
pkg/trunk/mechanism/mechanism_model/src/joint.cpp
pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp
pkg/trunk/robot_descriptions/wg_robot_description_parser/CMakeLists.txt
pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml
pkg/trunk/robot_descriptions/wg_robot_description_parser/test/test_cpp_parser.cpp
pkg/trunk/util/math_utils/include/math_utils/MathExpression.h
pkg/trunk/util/math_utils/src/MathExpression.cpp
pkg/trunk/util/string_utils/include/string_utils/string_utils.h
pkg/trunk/util/string_utils/src/string_utils.cpp
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h 2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h 2008-08-19 21:52:35 UTC (rev 3273)
@@ -35,6 +35,8 @@
#include <gazebo/Model.hh>
#include "hardware_interface/hardware_interface.h"
#include "mechanism_control/mechanism_control.h"
+#include "mechanism_model/robot.h"
+#include "tinyxml/tinyxml.h"
namespace gazebo
@@ -63,15 +65,13 @@
MechanismControl mc_;
MechanismControlNode mcn_;
- // Each joint in joints_ corresponds to the joint with the same
- // index in mech_joints_. The mech_joints_ vector exists so that
- // each transmission has a mechanism::Joint to write to, because it
- // would be best if the transmissions did not depend on Gazebo
- // objects.
+ TiXmlDocument config_;
+
+ // The fake model helps Gazebo run the transmissions backwards, so
+ // that it can figure out what its joints should do based on the
+ // actuator values.
+ mechanism::Robot fake_model_;
std::vector<gazebo::Joint*> joints_;
- std::vector<mechanism::Joint*> mech_joints_;
- std::vector<mechanism::Transmission*> transmissions_;
- std::vector<std::string> actuator_names_;
};
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2008-08-19 21:52:35 UTC (rev 3273)
@@ -25,6 +25,7 @@
<depend package="hardware_interface" />
<depend package="mechanism_control" />
<depend package="wg_robot_description_parser" />
+<depend package="tinyxml" />
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib" />
</export>
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp 2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp 2008-08-19 21:52:35 UTC (rev 3273)
@@ -32,6 +32,7 @@
#include <iostream>
#include <math.h>
#include <unistd.h>
+#include <set>
#include <stl_utils/stl_utils.h>
#include <gazebo/Global.hh>
#include <gazebo/XMLConfig.hh>
@@ -42,18 +43,20 @@
#include <gazebo/gazebo.h>
#include <gazebo/GazeboError.hh>
#include <gazebo/ControllerFactory.hh>
+#include <urdf/parser.h>
namespace gazebo {
GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_actuators", GazeboActuators);
GazeboActuators::GazeboActuators(Entity *parent)
- : Controller(parent), hw_(0), mc_(&hw_), mcn_(&mc_)
+ : Controller(parent), hw_(0), mc_(&hw_), mcn_(&mc_), fake_model_("fake")
{
- this->parent_model_ = dynamic_cast<Model*>(this->parent);
+ fake_model_.hw_ = &hw_;
+ this->parent_model_ = dynamic_cast<Model*>(this->parent);
- if (!this->parent_model_)
- gzthrow("GazeboActuators controller requires a Model as its parent");
+ if (!this->parent_model_)
+ gzthrow("GazeboActuators controller requires a Model as its parent");
}
GazeboActuators::~GazeboActuators()
@@ -63,75 +66,71 @@
void GazeboActuators::LoadChild(XMLConfigNode *node)
{
- XMLConfigNode *xit; // XML iterator
-
- // Reads the joints out of Gazebo's config.
- std::map<std::string,int> joint_lookup;
- for (xit = node->GetChild("joint"); xit; xit = xit->GetNext("joint"))
+ XMLConfigNode *robot = node->GetChild("robot");
+ if (!robot)
{
- std::string joint_name = xit->GetString("name", "", 1);
- gazebo::Joint *joint = parent_model_->GetJoint(joint_name);
- assert(joint != NULL);
- joints_.push_back(joint);
- mech_joints_.push_back(new mechanism::Joint);
-
- joint_lookup.insert(std::pair<std::string,int>(joint_name, joints_.size()-1));
+ fprintf(stderr, "Error loading gazebo_actuators config: no robot element\n");
+ return;
}
- // Reads the transmission information from the config.
- for (xit = node->GetChild("transmission"); xit; xit = xit->GetNext("transmission"))
+ // TODO: should look for the file relative to the path of the world file.
+ std::string filename = robot->GetString("filename", "", 1);
+ printf("Loading %s\n", filename.c_str());
+
+ TiXmlDocument doc(filename);
+ doc.LoadFile();
+ assert(doc.RootElement());
+ urdf::normalizeXml(doc.RootElement());
+
+ // Pulls out the list of actuators used in the robot configuration.
+ struct GetActuators : public TiXmlVisitor
{
- if (0 == strcmp("SimpleTransmission", xit->GetString("type", "", 0).c_str()))
+ std::set<std::string> actuators;
+ virtual bool VisitEnter(const TiXmlElement &elt, const TiXmlAttribute *)
{
- // Looks up the joint by name
- XMLConfigNode *joint_node = xit->GetChild("joint");
- assert(joint_node != NULL);
- std::map<std::string,int>::iterator jit = joint_lookup.find(joint_node->GetString("name", "", 1));
- if (jit == joint_lookup.end())
- {
- // TODO: report: Could not find the joint named xxxx
- continue;
- }
+ if (elt.ValueStr() == std::string("actuator") && elt.Attribute("name"))
+ actuators.insert(elt.Attribute("name"));
+ return true;
+ }
+ } get_actuators;
+ doc.RootElement()->Accept(&get_actuators);
- // Creates the actuator
- XMLConfigNode *actuator_node = xit->GetChild("actuator");
- assert(actuator_node != NULL);
- Actuator *actuator = new Actuator;
- hw_.actuators_.push_back(actuator);
- actuator_names_.push_back(actuator_node->GetString("name", "", 1));
+ // Places the found actuators into the hardware interface.
+ std::set<std::string>::iterator it;
+ for (it = get_actuators.actuators.begin(); it != get_actuators.actuators.end(); ++it)
+ {
+ hw_.actuators_.push_back(new Actuator(*it));
+ }
- // Creates the transmission
- transmissions_.push_back(
- new mechanism::SimpleTransmission(mech_joints_[jit->second], actuator,
- xit->GetDouble("mechanicalReduction", 0.0, 1),
- xit->GetDouble("motorTorqueConstant", 0.0, 1),
- xit->GetDouble("pulsesPerRevolution", 0.0, 1)));
+ // Initializes the fake model (for running the transmissions backwards).
+ fake_model_.initXml(doc.RootElement());
- }
+ // The gazebo joints and mechanism joints should match up.
+ for (unsigned int i = 0; i < fake_model_.joints_.size(); ++i)
+ {
+ std::string joint_name = fake_model_.joints_[i]->name_;
+ gazebo::Joint *joint = parent_model_->GetJoint(joint_name);
+ if (joint)
+ joints_.push_back(joint);
else
{
- // TODO: report: Unknown transmission type
- continue;
+ fprintf(stderr, "Gazebo does not know about a joint named \"%s\"\n", joint_name.c_str());
+ joints_.push_back(NULL);
}
}
+
+ hw_.current_time_ = Simulator::Instance()->GetSimTime();
+ mcn_.initXml(doc.RootElement());
}
void GazeboActuators::InitChild()
{
- // Registers the actuators with MechanismControl
- assert(actuator_names_.size() == hw_.actuators_.size());
- for (unsigned int i = 0; i < actuator_names_.size(); ++i)
- mc_.registerActuator(actuator_names_[i], i);
-
-
- // TODO: mc.init();
-
hw_.current_time_ = Simulator::Instance()->GetSimTime();
}
void GazeboActuators::UpdateChild()
{
- assert(joints_.size() == mech_joints_.size());
+ assert(joints_.size() == fake_model_.joints_.size());
//--------------------------------------------------
// Pushes out simulation state
@@ -140,22 +139,23 @@
// Copies the state from the gazebo joints into the mechanism joints.
for (unsigned int i = 0; i < joints_.size(); ++i)
{
+ if (!joints_[i])
+ continue;
assert(joints_[i]->GetType() == Joint::HINGE);
HingeJoint *hj = (HingeJoint*)joints_[i];
- mech_joints_[i]->position_ = hj->GetAngle();
- mech_joints_[i]->velocity_ = hj->GetAngleRate();
- mech_joints_[i]->applied_effort_ = mech_joints_[i]->commanded_effort_;
+ fake_model_.joints_[i]->position_ = hj->GetAngle();
+ fake_model_.joints_[i]->velocity_ = hj->GetAngleRate();
+ fake_model_.joints_[i]->applied_effort_ = fake_model_.joints_[i]->commanded_effort_;
}
// Reverses the transmissions to propagate the joint position into the actuators.
- for (unsigned int i = 0; i < transmissions_.size(); ++i)
- transmissions_[i]->propagatePositionBackwards();
+ for (unsigned int i = 0; i < fake_model_.transmissions_.size(); ++i)
+ fake_model_.transmissions_[i]->propagatePositionBackwards();
//--------------------------------------------------
// Runs Mechanism Control
//--------------------------------------------------
hw_.current_time_ = Simulator::Instance()->GetSimTime();
- //mc_.update();
mcn_.update();
//--------------------------------------------------
@@ -163,15 +163,17 @@
//--------------------------------------------------
// Reverses the transmissions to propagate the actuator commands into the joints.
- for (unsigned int i = 0; i < transmissions_.size(); ++i)
- transmissions_[i]->propagateEffortBackwards();
+ for (unsigned int i = 0; i < fake_model_.transmissions_.size(); ++i)
+ fake_model_.transmissions_[i]->propagateEffortBackwards();
// Copies the commands from the mechanism joints into the gazebo joints.
for (unsigned int i = 0; i < joints_.size(); ++i)
{
+ if (!joints_[i])
+ continue;
assert(joints_[i]->GetType() == Joint::HINGE);
HingeJoint *hj = (HingeJoint*)joints_[i];
- hj->SetTorque(mech_joints_[i]->commanded_effort_);
+ hj->SetTorque(fake_model_.joints_[i]->commanded_effort_);
}
}
Modified: pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h
===================================================================
--- pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h 2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h 2008-08-19 21:52:35 UTC (rev 3273)
@@ -35,6 +35,8 @@
#ifndef HARDWARE_INTERFACE_H
#define HARDWARE_INTERFACE_H
+#include <string>
+#include <vector>
#include <stl_utils/stl_utils.h>
class ActuatorState{
@@ -81,6 +83,11 @@
class Actuator
{
public:
+ Actuator() {}
+ Actuator(const std::string &name) : name_(name) {}
+ ~Actuator() {}
+
+ std::string name_;
ActuatorState state_;
ActuatorCommand command_;
};
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-08-19 21:52:35 UTC (rev 3273)
@@ -70,7 +70,6 @@
// Non real-time functions
bool initXml(TiXmlElement* config);
- bool registerActuator(const std::string &name, int index);
void getControllerNames(std::vector<std::string> &v);
bool addController(controller::Controller *c, const std::string &name);
bool spawnController(const std::string &type, const std::string &name, TiXmlElement *config);
@@ -78,7 +77,7 @@
bool addJoint(mechanism::Joint* j);
bool addSimpleTransmission(mechanism::SimpleTransmission *st);
controller::Controller* getControllerByName(std::string name);
-
+
mechanism::Robot model_;
HardwareInterface *hw_;
@@ -127,19 +126,19 @@
mechanism_control::KillController::response &resp);
MechanismControl *mc_;
-
+
// Non-realtime thread for publishing state information.
pthread_t *state_publishing_thread_;
void statePublishingLoop(void);
bool state_publishing_loop_keep_running_;
static const double STATE_PUBLISHING_PERIOD = 0.1; // in seconds, higher rates are useless with the current speed of the simulator
-
+
// Structure for transfering the mechanism state between realtime and non-realtime.
pthread_mutex_t mechanism_state_lock_;
// Blocks the ros publishing loop until an update is received from HW loop.
bool mechanism_state_updated_;
- pthread_cond_t mechanism_state_updated_cond_;
+ pthread_cond_t mechanism_state_updated_cond_;
mechanism_control::MechanismState mechanism_state_;
void publishMechanismState(); // Not realtime safe
const char * const mechanism_state_topic_;
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-08-19 21:52:35 UTC (rev 3273)
@@ -43,57 +43,20 @@
{
}
-bool MechanismControl::registerActuator(const std::string &name, int index)
-{
- if (initialized_)
- return false;
-
- model_.actuators_lookup_.insert(Robot::IndexMap::value_type(name, index));
-
- return true;
-}
-
bool MechanismControl::initXml(TiXmlElement* config)
{
- bool successful = true;
ros::node *node = ros::node::instance();
- TiXmlElement *elt;
+ model_.initXml(config);
- // Construct the joints
- for (elt = config->FirstChildElement("joint"); elt; elt = elt->NextSiblingElement("joint"))
- {
- Joint *j = new Joint;
- model_.joints_.push_back(j);
- model_.joints_lookup_.insert(Robot::IndexMap::value_type(elt->Attribute("name"), model_.joints_.size() - 1));
- j->initXml(elt);
- }
-
- // Construct the transmissions
- elt = config->FirstChildElement("transmission");
- for (; elt; elt = elt->NextSiblingElement("transmission"))
- {
- const char *type = elt->Attribute("type");
- Transmission *t = TransmissionFactory::instance().create(type);
- if (t == NULL)
- node->log(ros::FATAL, "Unknown transmission type: %s\n", type);
- t->initXml(elt, &model_);
- model_.transmissions_.push_back(t);
- }
-
initialized_ = true;
- return successful;
+ return true;
}
bool MechanismControl::addJoint(Joint* j)
{
- bool successful = true;
-
- // add the joints
model_.joints_.push_back(j);
- model_.joints_lookup_.insert(Robot::IndexMap::value_type(j->name_, model_.joints_.size() - 1));
-
- return successful;
+ return true;
}
bool MechanismControl::addSimpleTransmission(SimpleTransmission *st)
@@ -130,11 +93,11 @@
// Propagates through the robot model.
for (unsigned int i = 0; i < model_.transmissions_.size(); ++i)
model_.transmissions_[i]->propagatePosition();
-
+
//zero all commands
for (unsigned int i = 0; i < model_.joints_.size(); ++i)
model_.joints_[i]->commanded_effort_= 0.0;
-
+
// TODO: update KDL model with new joint position/velocities
// Update all controllers
@@ -185,7 +148,7 @@
{
//Add controller to list of controllers in realtime-safe manner;
controllers_lock_.lock(); //This lock is only to prevent us from other non-realtime threads. The realtime thread may be spinning through the list of controllers while we are in here, so we need to keep that list always in a valid state. This is why we fully allocate and set up the controller before adding it into the list of active controllers.
-
+
bool spot_found = false;
for (int i = 0; i < MAX_NUM_CONTROLLERS; i++)
{
@@ -261,6 +224,9 @@
assert(mc != NULL);
assert(mechanism_state_topic_);
if ((node = ros::node::instance()) == NULL) {
+ int argc = 0;
+ char** argv = NULL;
+ ros::init(argc, argv);
node = new ros::node("mechanism_control", ros::node::DONT_HANDLE_SIGINT);
}
@@ -282,7 +248,7 @@
}
MechanismControlNode::~MechanismControlNode()
-{
+{
state_publishing_loop_keep_running_ = false;
}
@@ -297,7 +263,7 @@
void MechanismControlNode::update()
{
mc_->update();
-
+
// Attempts to lock the transfer structure
// If we get it, update it
if(pthread_mutex_trylock(&mechanism_state_lock_))
Modified: pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
===================================================================
--- pkg/trunk/mechanism/mechanism_model/CMakeLists.txt 2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/mechanism/mechanism_model/CMakeLists.txt 2008-08-19 21:52:35 UTC (rev 3273)
@@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
rospack(mechanism_model)
-rospack_add_library(mechanism_model src/simple_transmission.cpp src/joint.cpp)
+rospack_add_library(mechanism_model src/simple_transmission.cpp src/joint.cpp src/robot.cpp)
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h 2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h 2008-08-19 21:52:35 UTC (rev 3273)
@@ -31,6 +31,9 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
+/*
+ *
+ */
#ifndef JOINT_H
#define JOINT_H
@@ -40,8 +43,11 @@
class Joint{
public:
+ Joint() : commanded_effort_(0) {}
+ ~Joint() {}
+
void enforceLimits();
- void initXml(TiXmlElement *elt);
+ bool initXml(TiXmlElement *elt);
std::string name_;
int type_;
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h 2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h 2008-08-19 21:52:35 UTC (rev 3273)
@@ -45,13 +45,15 @@
#include "mechanism_model/transmission.h"
#include "hardware_interface/hardware_interface.h"
+class TiXmlElement;
+
namespace mechanism
{
class Robot
{
public:
- Robot(char *ns){}
+ Robot(const char *ns){}
~Robot()
{
@@ -59,28 +61,13 @@
deleteElements(&joints_);
}
+ bool initXml(TiXmlElement *root);
+
std::vector<Joint*> joints_;
std::vector<Transmission*> transmissions_;
- // Supports looking up joints and actuators by name. The IndexMap
- // structure maps the name of the item to its index in the vectors.
- typedef std::map<std::string,int> IndexMap;
- IndexMap joints_lookup_;
- IndexMap actuators_lookup_;
- Joint* getJoint(const std::string &name)
- {
- IndexMap::iterator it = joints_lookup_.find(name);
- if (it == joints_lookup_.end())
- return NULL;
- return joints_[it->second];
- }
- Actuator* getActuator(const std::string &name)
- {
- IndexMap::iterator it = actuators_lookup_.find(name);
- if (it == actuators_lookup_.end())
- return NULL;
- return hw_->actuators_[it->second];
- }
+ Joint* getJoint(const std::string &name);
+ Actuator* getActuator(const std::string &name);
HardwareInterface *hw_;
};
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h 2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h 2008-08-19 21:52:35 UTC (rev 3273)
@@ -61,7 +61,7 @@
virtual ~Transmission() {}
// Initialize transmission from XML data
- virtual void initXml(TiXmlElement *config, Robot *robot) = 0;
+ virtual bool initXml(TiXmlElement *config, Robot *robot) = 0;
// another way to initialize simple transmission
virtual void initTransmission(std::string transmission_name,std::string joint_name,std::string actuator_name,double mechanical_reduction,double motor_torque_constant,double pulses_per_revolution, Robot *robot) = 0;
@@ -88,7 +88,7 @@
SimpleTransmission(Joint *joint, Actuator *actuator, double mechanical_reduction, double motor_torque_constant, double pulses_per_revolution);
~SimpleTransmission() {}
- void initXml(TiXmlElement *config, Robot *robot);
+ bool initXml(TiXmlElement *config, Robot *robot);
void initTransmission(std::string transmission_name,std::string joint_name,std::string actuator_name,double mechanical_reduction,double motor_torque_constant,double pulses_per_revolution, Robot *robot);
std::string name_;
Modified: pkg/trunk/mechanism/mechanism_model/src/joint.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/joint.cpp 2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/mechanism/mechanism_model/src/joint.cpp 2008-08-19 21:52:35 UTC (rev 3273)
@@ -32,9 +32,10 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
+#include <mechanism_model/joint.h>
#include <map>
#include <string>
-#include <mechanism_model/joint.h>
+#include <cfloat>
using namespace std;
using namespace mechanism;
@@ -54,20 +55,48 @@
commanded_effort_ = min(max(commanded_effort_, -effort_limit_), effort_limit_);
}
-void Joint::initXml(TiXmlElement *elt)
+bool Joint::initXml(TiXmlElement *elt)
{
- TiXmlElement *min = elt->FirstChildElement("limitMin");
- TiXmlElement *max = elt->FirstChildElement("limitMax");
- joint_limit_min_ = min ? atof(min->GetText()) : 0;
- joint_limit_max_ = max ? atof(max->GetText()) : 0;
- effort_limit_ = atof(elt->FirstChildElement("effortLimit")->GetText());
- velocity_limit_ = atof(elt->FirstChildElement("velocityLimit")->GetText());
+ const char *name = elt->Attribute("name");
+ if (!name)
+ {
+ fprintf(stderr, "Error: unnamed joint found\n");
+ return false;
+ }
+ name_ = name;
- type_ = g_type_map[elt->Attribute("type")];
- // If type is revolute and limits aren't set, then it is continuous
- if (type_ == JOINT_ROTARY && min == NULL && max == NULL)
+ const char *type = elt->Attribute("type");
+ if (!type)
{
- type_ = JOINT_CONTINUOUS;
+ fprintf(stderr, "Error: Joint \"%s\" has no type.\n", name_.c_str());
+ return false;
}
+ type_ = g_type_map[type];
+
+ TiXmlElement *limits = elt->FirstChildElement("limit");
+ if (limits)
+ {
+ if (limits->QueryDoubleAttribute("effort", &effort_limit_) != TIXML_SUCCESS)
+ effort_limit_ = 0.0;
+ if (limits->QueryDoubleAttribute("velocity", &velocity_limit_) != TIXML_SUCCESS)
+ velocity_limit_ = 0.0;
+
+
+ int min_ret = limits->QueryDoubleAttribute("min", &joint_limit_min_);
+ int max_ret = limits->QueryDoubleAttribute("max", &joint_limit_max_);
+
+ if (type_ == JOINT_ROTARY && min_ret == TIXML_NO_ATTRIBUTE && max_ret == TIXML_NO_ATTRIBUTE)
+ {
+ type_ = JOINT_CONTINUOUS;
+ }
+ }
+ else
+ {
+ if (type_ == JOINT_ROTARY)
+ type_ = JOINT_CONTINUOUS;
+ effort_limit_ = DBL_MAX;
+ }
+
+ return true;
}
Modified: pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp 2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp 2008-08-19 21:52:35 UTC (rev 3273)
@@ -50,13 +50,30 @@
joint_ = joint;
}
-void SimpleTransmission::initXml(TiXmlElement *elt, Robot *robot)
+bool SimpleTransmission::initXml(TiXmlElement *elt, Robot *robot)
{
- joint_ = robot->getJoint(elt->FirstChildElement("joint")->Attribute("name"));
- actuator_ = robot->getActuator(elt->FirstChildElement("actuator")->Attribute("name"));
+ TiXmlElement *jel = elt->FirstChildElement("joint");
+ const char *joint_name = jel ? jel->Attribute("name") : NULL;
+ joint_ = joint_name ? robot->getJoint(joint_name) : NULL;
+ if (!joint_)
+ {
+ fprintf(stderr, "SimpleTransmission could not find joint named \"%s\"\n", joint_name);
+ return false;
+ }
+
+ TiXmlElement *ael = elt->FirstChildElement("actuator");
+ const char *actuator_name = ael ? ael->Attribute("name") : NULL;
+ actuator_ = actuator_name ? robot->getActuator(actuator_name) : NULL;
+ if (!actuator_)
+ {
+ fprintf(stderr, "SimpleTransmission could not find actuator named \"%s\"\n", actuator_name);
+ return false;
+ }
+
mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText()),
motor_torque_constant_ = atof(elt->FirstChildElement("motorTorqueConstant")->GetText()),
pulses_per_revolution_ = atof(elt->FirstChildElement("pulsesPerRevolution")->GetText());
+ return true;
}
void SimpleTransmission::initTransmission(std::string transmission_name,std::string joint_name,std::string actuator_name,double mechanical_reduction,double motor_torque_constant,double pulses_per_revolution, Robot *robot)
@@ -73,14 +90,16 @@
void SimpleTransmission::propagatePosition()
{
+ assert(joint_); assert(actuator_);
joint_->position_ = ((double)actuator_->state_.encoder_count_*2*M_PI)/(pulses_per_revolution_ * mechanical_reduction_);
joint_->velocity_ = ((double)actuator_->state_.encoder_velocity_*2*M_PI)/(pulses_per_revolution_ * mechanical_reduction_);
joint_->applied_effort_ = actuator_->state_.last_measured_current_ * (motor_torque_constant_ * mechanical_reduction_);
-
+
}
void SimpleTransmission::propagatePositionBackwards()
{
+ assert(joint_); assert(actuator_);
actuator_->state_.encoder_count_ = (int)(joint_->position_ * pulses_per_revolution_ * mechanical_reduction_ / (2*M_PI));
actuator_->state_.encoder_velocity_ = joint_->velocity_ * pulses_per_revolution_ * mechanical_reduction_ / (2*M_PI);
actuator_->state_.last_measured_current_ = joint_->applied_effort_ / (motor_torque_constant_ * mechanical_reduction_);
@@ -88,13 +107,14 @@
void SimpleTransmission::propagateEffort()
{
+ assert(joint_); assert(actuator_);
actuator_->command_.current_ = joint_->commanded_effort_/(motor_torque_constant_ * mechanical_reduction_);
actuator_->command_.enable_ = true;
-
+
}
void SimpleTransmission::propagateEffortBackwards()
{
+ assert(joint_); assert(actuator_);
joint_->commanded_effort_ = actuator_->command_.current_ * motor_torque_constant_ * mechanical_reduction_;
-
}
Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/CMakeLists.txt 2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/CMakeLists.txt 2008-08-19 21:52:35 UTC (rev 3273)
@@ -4,7 +4,7 @@
set(ROS_COMPILE_FLAGS "-W -Wall -Wextra")
-rospack_add_library(URDF src/urdf/URDF.cpp)
+rospack_add_library(URDF src/urdf/URDF.cpp src/parser.cpp)
rospack_add_executable(parse src/urdf/parse.cpp)
target_link_libraries(parse URDF)
rospack_add_executable(merge src/urdf/merge.cpp)
Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml 2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml 2008-08-19 21:52:35 UTC (rev 3273)
@@ -12,10 +12,11 @@
<depend package="tinyxml"/>
<depend package="math_utils"/>
<depend package="string_utils"/>
+ <depend package="stl_utils" />
<depend package="gtest"/>
-
+
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lURDF"/>
</export>
-
+
</package>
Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/test/test_cpp_parser.cpp
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/test/test_cpp_parser.cpp 2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/test/test_cpp_parser.cpp 2008-08-19 21:52:35 UTC (rev 3273)
@@ -1,13 +1,13 @@
/*********************************************************************
* 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
@@ -17,7 +17,7 @@
* * 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
@@ -33,8 +33,10 @@
*********************************************************************/
#include <urdf/URDF.h>
+#include <urdf/parser.h>
#include <gtest/gtest.h>
#include <cstdlib>
+#include <cmath>
#include <sstream>
#include <fstream>
#include <string>
@@ -50,12 +52,12 @@
URDF file;
file.loadFile("test/data/test1.xml");
EXPECT_TRUE(file.getErrorCount() == 0);
-
+
std::ofstream f("/tmp/test1.txt");
file.print(f);
f.close();
int result = runExternalProcess("diff", "test/data/test1.txt /tmp/test1.txt");
-
+
EXPECT_TRUE(result == 0);
}
@@ -64,12 +66,12 @@
URDF file;
file.loadFile("test/data/test2.xml");
EXPECT_TRUE(file.getErrorCount() == 0);
-
+
std::ofstream f("/tmp/test2.txt");
file.print(f);
f.close();
int result = runExternalProcess("diff", "test/data/test2.txt /tmp/test2.txt");
-
+
EXPECT_TRUE(result == 0);
}
@@ -84,17 +86,93 @@
file.print(f);
f.close();
int result = runExternalProcess("diff", "test/data/test3.txt /tmp/test3.txt");
-
+
EXPECT_TRUE(result == 0);
}
TEST(URDF, Valgrind)
{
int result = runExternalProcess("./run_valgrind.py", "./parse test/data/test3.xml");
-
+
EXPECT_TRUE(result == 0);
}
+//------------------------------------------------------------------------------
+// Normalization
+
+bool xmlMatches(TiXmlElement *a, TiXmlElement *b)
+{
+ if (a == NULL && b == NULL)
+ return true;
+ if (a == NULL || b == NULL)
+ return false;
+
+ if (a->ValueStr() != b->ValueStr())
+ return false;
+
+ for (TiXmlAttribute *att = a->FirstAttribute(); att; att = att->Next())
+ {
+ std::string va = att->ValueStr();
+ std::string vb = b->Attribute(att->Name());
+
+ if (atof(va.c_str()) != 0)
+ {
+ if (fabs(atof(va.c_str()) - atof(vb.c_str())) > 0.00000001)
+ return false;
+ }
+ else if (att->ValueStr() != b->Attribute(att->Name()))
+ return false;
+ }
+
+ if (!xmlMatches(a->FirstChildElement(), b->FirstChildElement()))
+ return false;
+ if (!xmlMatches(a->NextSiblingElement(), b->NextSiblingElement()))
+ return false;
+
+ return true;
+}
+
+TEST(URDF, NormalizeASimpleDoc)
+{
+ TiXmlDocument xml, desired;
+ xml.Parse("<should do='nothing'>hopefully</should>");
+ desired.Parse("<should do='nothing'>hopefully</should>");
+ EXPECT_TRUE(urdf::normalizeXml(xml.RootElement()));
+ EXPECT_TRUE(xmlMatches(xml.RootElement(), desired.RootElement()));
+}
+
+TEST(URDF, NormalizeOneConstant)
+{
+ TiXmlDocument xml, desired;
+ xml.Parse("<x><const name='huzzah' value='3.0' /><foo bar='huzzah' /></x>");
+ desired.Parse("<x><foo bar='3.0' /></x>");
+ EXPECT_TRUE(urdf::normalizeXml(xml.RootElement()));
+ EXPECT_TRUE(xmlMatches(xml.RootElement(), desired.RootElement()));
+}
+
+TEST(URDF, NormalizeOneBlock)
+{
+ TiXmlDocument xml, desired;
+ xml.Parse("<x><const_block name='foo'><bar meaning='nothing' /></const_block> \
+<insert_const_block name='foo' /></x>");
+ desired.Parse("<x><bar meaning='nothing' /></x>");
+ EXPECT_TRUE(urdf::normalizeXml(xml.RootElement()));
+ EXPECT_TRUE(xmlMatches(xml.RootElement(), desired.RootElement()));
+}
+
+TEST(URDF, NormalizeAnExpressionWithConstants)
+{
+ TiXmlDocument xml, desired;
+ xml.Parse("<x> \
+<const name='aaa' value='4.5'/> \
+<const name='bbb' value='7.3'/> \
+<const name='ccc' value='2.0'/> \
+<y><z foo='(aaa*bbb)/ccc' /></y></x>");
+ desired.Parse("<x><y><z foo='16.425' /></y></x>");
+ EXPECT_TRUE(urdf::normalizeXml(xml.RootElement()));
+ EXPECT_TRUE(xmlMatches(xml.RootElement(), desired.RootElement()));
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
Modified: pkg/trunk/util/math_utils/include/math_utils/MathExpression.h
===================================================================
--- pkg/trunk/util/math_utils/include/math_utils/MathExpression.h 2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/util/math_utils/include/math_utils/MathExpression.h 2008-08-19 21:52:35 UTC (rev 3273)
@@ -53,7 +53,11 @@
* mathematical operators. In addition to floating point constants,
* this function allows the use of named constants, if a callback is
* provided to evaluate those named constants. A data pointer for the
- * callback is provided for convenience. */
+
+ * callback is provided for convenience.
+ *
+ * Returns nan on error.
+ */
double EvaluateMathExpression(const char *expression, ExpressionVariableFn var = NULL, void *data = NULL);
double EvaluateMathExpression(const std::string &expression, ExpressionVariableFn var = NULL, void *data = NULL);
Modified: pkg/trunk/util/math_utils/src/MathExpression.cpp
===================================================================
--- pkg/trunk/util/math_utils/src/MathExpression.cpp 2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/util/math_utils/src/MathExpression.cpp 2008-08-19 21:52:35 UTC (rev 3273)
@@ -111,18 +111,24 @@
{
bool variable = false;
for (unsigned int i = 0 ; i < exp.size() ; ++i)
+ {
if (!((exp[i] <= '9' && exp[i] >= '0') || exp[i] == '+' || exp[i] == '-' || exp[i] == '.'))
{
variable = true;
break;
}
+ }
+
if (variable)
{
if (var)
return var(data, exp);
+ return NAN;
}
else
+ {
return atof(exp.c_str());
+ }
}
}
else
@@ -130,18 +136,28 @@
unsigned int pos = ops[0];
std::string exp1 = exp.substr(0, pos);
std::string exp2 = exp.substr(pos + 1);
+ double val1, val2;
+ val1 = EvaluateMathExpression(exp1, var, data);
+ val2 = EvaluateMathExpression(exp2, var, data);
+
+ // Hack: handles unary minus
+ if (exp1.size() == 0)
+ val1 = 0.0;
+
switch (exp[pos])
{
case '+':
- return EvaluateMathExpression(exp1, var, data) + EvaluateMathExpression(exp2, var, data);
+ return val1 + val2;
case '-':
- return EvaluateMathExpression(exp1, var, data) - EvaluateMathExpression(exp2, var, data);
+ return val1 - val2;
case '*':
- return EvaluateMathExpression(exp1, var, data) * EvaluateMathExpression(exp2, var, data);
+ return val1 * val2;
case '/':
- return EvaluateMathExpression(exp1, var, data) / EvaluateMathExpression(exp2, var, data);
+ return val1 / val2;
+ default:
+ return NAN;
}
}
- return 0.0;
+ return NAN;
}
Modified: pkg/trunk/util/string_utils/include/string_utils/string_utils.h
===================================================================
--- pkg/trunk/util/string_utils/include/string_utils/string_utils.h 2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/util/string_utils/include/string_utils/string_utils.h 2008-08-19 21:52:35 UTC (rev 3273)
@@ -7,19 +7,22 @@
namespace string_utils
{
- void split(const std::string &str,
- std::vector<std::string> &token_vec,
- const std::string &delim);
-
- std::string trim(const std::string &str);
-
- template<typename T>
- static inline std::string convert2str(const T &value)
- {
- std::stringstream ss;
- ss << value;
- return ss.str();
- }
+void split(const std::string &str,
+ std::vector<std::string> &token_vec,
+ const std::string &delim);
+
+// Default version, which splits on whitespace.
+void split(const std::string &str, std::vector<std::string> &tokens);
+
+std::string trim(const std::string &str);
+
+template<typename T>
+static inline std::string convert2str(const T &value)
+{
+ std::stringstream ss;
+ ss << value;
+ return ss.str();
}
+}
#endif
Modified: pkg/trunk/util/string_utils/src/string_utils.cpp
===================================================================
--- pkg/trunk/util/string_utils/src/string_utils.cpp 2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/util/string_utils/src/string_utils.cpp 2008-08-19 21:52:35 UTC (rev 3273)
@@ -12,6 +12,29 @@
t.push_back(s.substr(start));
}
+void string_utils::split(const std::string &str, std::vector<std::string> &tokens)
+{
+ std::size_t start = 0, i;
+
+ while (true)
+ {
+ while (start < str.size() && isspace(str[start]))
+ ++start;
+ if (start == str.size())
+ return;
+
+ i = start;
+ while (i < str.size() && !isspace(str[i]))
+ ++i;
+
+ tokens.push_back(str.substr(start, i - start));
+ if (i == str.size())
+ return;
+
+ start = i;
+ }
+}
+
std::string string_utils::trim(const std::string &str)
{
std::string res = str;
@@ -27,5 +50,5 @@
res[0] == '\n' ||
res[0] == '\r'))
res.erase(0, 1);
- return res;
+ return res;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|