Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2244] pkg/trunk
|
From: <stu...@us...> - 2008-07-29 17:14:55
|
Revision: 2244
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2244&view=rev
Author: stuglaser
Date: 2008-07-29 17:15:03 +0000 (Tue, 29 Jul 2008)
Log Message:
-----------
Added initXml method to Controller base class. Calling it in mechanism control.
Modified Paths:
--------------
pkg/trunk/controllers/genericControllers/include/genericControllers/Controller.h
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
Modified: pkg/trunk/controllers/genericControllers/include/genericControllers/Controller.h
===================================================================
--- pkg/trunk/controllers/genericControllers/include/genericControllers/Controller.h 2008-07-29 07:41:01 UTC (rev 2243)
+++ pkg/trunk/controllers/genericControllers/include/genericControllers/Controller.h 2008-07-29 17:15:03 UTC (rev 2244)
@@ -9,8 +9,9 @@
*/
/***************************************************/
+#include <mechanism_model/robot.h>
+
class TiXmlElement;
-class Robot;
namespace controller
{
@@ -46,7 +47,7 @@
virtual ~Controller() {}
virtual void update(void) {}
virtual void init(void) {}
- virtual void initXml(Robot *robot, TiXmlElement *config) {}
+ virtual void initXml(mechanism::Robot *robot, TiXmlElement *config) {}
private:
};
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-07-29 07:41:01 UTC (rev 2243)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-07-29 17:15:03 UTC (rev 2244)
@@ -37,19 +37,19 @@
#include <rosthread/mutex.h>
#include <genericControllers/Controller.h>
-typedef controller::Controller* (*ControllerAllocator)(const std::string&);
+typedef controller::Controller* (*ControllerAllocator)();
class MechanismControl {
public:
MechanismControl(HardwareInterface *hw);
~MechanismControl();
- bool init();
+ bool init(TiXmlElement* config);
void update(); //Must be realtime safe
bool registerActuator(const std::string &name, int index);
void registerControllerType(const std::string& type, ControllerAllocator f);
- bool spawnController(const char* type, const char* ns);
+ bool spawnController(const char* type, TiXmlElement* config);
Actuator* getActuator(const std::string& name);
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-07-29 07:41:01 UTC (rev 2243)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-07-29 17:15:03 UTC (rev 2244)
@@ -2,32 +2,34 @@
///////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2008, Willow Garage, Inc.
//
-// Redistribution and use in source and binary forms, with or without
+// 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,
+// * 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
+// * 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 Willow Garage, Inc. nor the names of its
-// contributors may be used to endorse or promote products derived from
+// * Neither the name of Willow Garage, Inc. 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
+//
+// 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.
//////////////////////////////////////////////////////////////////////////////
#include "mechanism_control/mechanism_control.h"
+using namespace mechanism;
+
MechanismControl::MechanismControl(HardwareInterface *hw)
: initialized_(0), model_((char*)"robot"), hw_(hw)
{
@@ -46,48 +48,90 @@
return true;
}
-bool MechanismControl::init() { // TODO: should take in a node of xml?
+bool MechanismControl::init(TiXmlElement* config) {
+
+ bool successful = true;
+
// Creates:
// - joints
// - transmissions
- return true;
+ TiXmlElement *elt;
+
+ // Constructs the joints
+ std::map<std::string, Joint*> joint_map;
+ for (elt = config->FirstChildElement("joint"); elt; elt = elt->NextSiblingElement("joint"))
+ {
+ Joint *j = new Joint;
+ model_.joints_.push_back(j);
+ joint_map[elt->Attribute("name")] = j;
+ j->jointLimitMin = atof(elt->FirstChildElement("limitMin")->GetText());
+ j->jointLimitMax = atof(elt->FirstChildElement("limitMax")->GetText());
+ j->effortLimit = atof(elt->FirstChildElement("effortLimit")->GetText());
+ j->velocityLimit = atof(elt->FirstChildElement("velocityLimit")->GetText());
+ }
+
+ // Constructs the transmissions
+ elt = config->FirstChildElement("transmission");
+ for (; elt; elt = elt->NextSiblingElement("transmission"))
+ {
+ if (0 == strcmp("SimpleTransmission", elt->Attribute("type")))
+ {
+ // Looks up the joint and the actuator used by the transmission.
+ std::map<std::string,Joint*>::iterator joint_it =
+ joint_map.find(elt->FirstChildElement("joint")->Attribute("name"));
+ ActuatorMap::iterator actuator_it =
+ actuators_.find(elt->FirstChildElement("actuator")->Attribute("name"));
+ if (joint_it == joint_map.end())
+ {
+ // TODO: report: The joint was not declared in the XML file
+ continue;
+ }
+ if (actuator_it == actuators_.end())
+ {
+ // TODO: report: The actuator was not registered with mechanism control.
+ continue;
+ }
+
+ Transmission *tr = new SimpleTransmission
+ (joint_it->second, &hw_->actuator[actuator_it->second],
+ atof(elt->FirstChildElement("mechanicalReduction")->Value()),
+ atof(elt->FirstChildElement("motorTorqueConstant")->Value()),
+ atof(elt->FirstChildElement("pulsesPerRevolution")->Value()));
+ model_.transmissions_.push_back(tr);
+ }
+ else
+ {
+ // TODO: report: Unknown transmission type
+ successful = false;
+ }
+ }
+
+ initialized_ = true;
+ return successful;
}
// Must be realtime safe.
void MechanismControl::update() {
- // Propogates actuator information into the robot model.
- // Propogates through the robot model.
- // Calls update on all of the plugins.
- // Performs safety checks on the commands.
- // Propogates commands back into the actuators.
+ // Propagates through the robot model.
+ for (unsigned int i = 0; i < model_.transmissions_.size(); ++i)
+ model_.transmissions_[i]->propagatePosition();
-#if 0
- //Clear actuator commands
+ // TODO: update KDL model with new joint position/velocities
- //Process robot model transmissions
- for(int i = 0; i < r->numTransmissions; i++){
- r->transmission[i].propagatePosition();
- }
-
- //update KDL model with new joint position/velocities
- //
-
//update all controllers
- for(int i = 0; i < MAX_NUM_CONTROLLERS; i++){
- if(controller[i] != NULL){
- controller[i]->update();
- }
+ for(int i = 0; i < MAX_NUM_CONTROLLERS; ++i){
+ if(controllers_[i] != NULL)
+ controllers_[i]->update();
}
- for(int i = 0; i < r->numJoints; i++){
- r->joint[i].enforceLimits();
- }
+ // Performs safety checks on the commands.
+ for (unsigned int i = 0; i < model_.joints_.size(); ++i)
+ model_.joints_[i]->enforceLimits();
- for(int i = 0; i < r->numTransmissions; i++){
- r->transmission[i].propagateEffort();
- }
-#endif
+ // Propagates commands back into the actuators.
+ for (unsigned int i = 0; i < model_.transmissions_.size(); ++i)
+ model_.transmissions_[i]->propagateEffort();
}
@@ -96,10 +140,12 @@
controller_library_.insert(std::pair<std::string,ControllerAllocator>(type, f));
}
-bool MechanismControl::spawnController(const char *type, const char *ns){
+bool MechanismControl::spawnController(const char *type, TiXmlElement *config)
+{
controller::Controller *c;
ControllerAllocator f = controller_library_[type];
- c = f(ns);
+ c = f();
+ c->initXml(&model_, config);
//At this point, the controller is fully initialized and has created the ROS interface, etc.
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
