Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3637] pkg/trunk
|
From: <stu...@us...> - 2008-08-26 20:52:29
|
Revision: 3637
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3637&view=rev
Author: stuglaser
Date: 2008-08-26 20:52:36 +0000 (Tue, 26 Aug 2008)
Log Message:
-----------
Parses kinematic chains and performs forward kinematics during update loop.
Modified Paths:
--------------
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/link.h
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
pkg/trunk/mechanism/mechanism_model/manifest.xml
pkg/trunk/mechanism/mechanism_model/src/link.cpp
pkg/trunk/mechanism/mechanism_model/src/robot.cpp
pkg/trunk/robot_descriptions/wg_robot_description_parser/include/urdf/parser.h
pkg/trunk/robot_descriptions/wg_robot_description_parser/src/parser.cpp
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-08-26 20:39:41 UTC (rev 3636)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-08-26 20:52:36 UTC (rev 3637)
@@ -91,13 +91,13 @@
// Propagates through the robot model.
for (unsigned int i = 0; i < model_.transmissions_.size(); ++i)
model_.transmissions_[i]->propagatePosition();
+ for (unsigned int i = 0; i < model_.chains_.size(); ++i)
+ model_.chains_[i]->propagateFK();
- //zero all commands
+ // Zeros 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
for (int i = 0; i < MAX_NUM_CONTROLLERS; ++i)
{
Modified: pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
===================================================================
--- pkg/trunk/mechanism/mechanism_model/CMakeLists.txt 2008-08-26 20:39:41 UTC (rev 3636)
+++ pkg/trunk/mechanism/mechanism_model/CMakeLists.txt 2008-08-26 20:52:36 UTC (rev 3637)
@@ -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 src/robot.cpp src/link.cpp)
+rospack_add_library(mechanism_model src/simple_transmission.cpp src/joint.cpp src/robot.cpp src/link.cpp src/chain.cpp)
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/link.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/link.h 2008-08-26 20:39:41 UTC (rev 3636)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/link.h 2008-08-26 20:52:36 UTC (rev 3637)
@@ -36,6 +36,7 @@
#include "mechanism_model/joint.h"
#include <vector>
+#include <kdl/frames.hpp>
namespace mechanism {
@@ -59,6 +60,8 @@
Joint *joint_;
std::vector<Link*> children_;
+ KDL::Frame frame_;
+
private:
enum InitState {INIT_XML, CREATE_TREE_LINKS, INITIALIZED};
InitState init_state_;
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h 2008-08-26 20:39:41 UTC (rev 3636)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h 2008-08-26 20:52:36 UTC (rev 3637)
@@ -31,8 +31,27 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-//The robot model is populated by the control code infrastructure and used by all the controllers to read mechanism state and command mechanism motion.
+/*
+ * The robot model tracks the state of the robot.
+ *
+ * State path:
+ * +---------------+ +--------+
+ * Actuators --> | Transmissions | --> Joints --> | Chains | --> Links
+ * +---------------+ +--------+
+ *
+ * The actuators, joints, and links, hold the state information. The
+ * actuators hold the encoder info, the joints hold the joint angles
+ * and velocities, and the links hold the frame transforms of the body
+ * segments.
+ *
+ * The transmissions and chains are for propagating the state through
+ * the model, and they themselves do not hold any information on the
+ * robot's current state.
+ *
+ * Author: Stuart Glaser
+ */
+
#ifndef ROBOT_H
#define ROBOT_H
@@ -43,6 +62,7 @@
#include "mechanism_model/link.h"
#include "mechanism_model/joint.h"
#include "mechanism_model/transmission.h"
+#include "mechanism_model/chain.h"
#include "hardware_interface/hardware_interface.h"
class TiXmlElement;
@@ -64,16 +84,16 @@
bool initXml(TiXmlElement *root);
+ HardwareInterface *hw_; // Holds the array of actuators
+ std::vector<Transmission*> transmissions_;
std::vector<Joint*> joints_;
- std::vector<Transmission*> transmissions_;
+ std::vector<Chain*> chains_;
std::vector<Link*> links_;
Joint* getJoint(const std::string &name);
Actuator* getActuator(const std::string &name);
Link* getLink(const std::string &name);
- HardwareInterface *hw_;
-
// For debugging
void printLinkTree();
};
Modified: pkg/trunk/mechanism/mechanism_model/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_model/manifest.xml 2008-08-26 20:39:41 UTC (rev 3636)
+++ pkg/trunk/mechanism/mechanism_model/manifest.xml 2008-08-26 20:52:36 UTC (rev 3637)
@@ -8,6 +8,8 @@
<depend package="stl_utils" />
<depend package="misc_utils" />
<depend package="tinyxml" />
+<depend package="wg_robot_description_parser" />
+<depend package="kdl" />
<url>http://pr.willowgarage.com</url>
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lmechanism_model -Wl,-rpath,${prefix}/lib"/>
Modified: pkg/trunk/mechanism/mechanism_model/src/link.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/link.cpp 2008-08-26 20:39:41 UTC (rev 3636)
+++ pkg/trunk/mechanism/mechanism_model/src/link.cpp 2008-08-26 20:52:36 UTC (rev 3637)
@@ -40,6 +40,7 @@
#include <map>
#include <string>
#include "mechanism_model/robot.h"
+#include "urdf/parser.h"
namespace mechanism {
@@ -64,6 +65,7 @@
}
name_ = std::string(name);
+ // Parent
TiXmlElement *p = config->FirstChildElement("parent");
const char *parent_name = p ? p->Attribute("name") : NULL;
if (!parent_name)
@@ -73,16 +75,47 @@
}
parent_name_ = std::string(parent_name);
+ // Joint
TiXmlElement *j = config->FirstChildElement("joint");
const char *joint_name = j ? j->Attribute("name") : NULL;
joint_ = joint_name ? robot->getJoint(joint_name) : NULL;
if (!joint_)
{
- fprintf(stderr, "Error: Link \"%s\" could not find the joint named \"%s\"\n", name_.c_str(), joint_name);
+ fprintf(stderr, "Error: Link \"%s\" could not find the joint named \"%s\"\n",
+ name_.c_str(), joint_name);
return false;
}
- // TODO: parse origin info
+ // Origin
+ TiXmlElement *o = config->FirstChildElement("origin");
+ if (!o)
+ {
+ fprintf(stderr, "Error: Link \"%s\" has no origin element\n", name_.c_str());
+ return false;
+ }
+ std::vector<double> xyz, rpy;
+ if (!urdf::queryVectorAttribute(o, "xyz", &xyz))
+ {
+ fprintf(stderr, "Error: Link \"%s\"'s origin has no xyz attribute\n", name_.c_str());
+ return false;
+ }
+ if (!urdf::queryVectorAttribute(o, "rpy", &rpy))
+ {
+ fprintf(stderr, "Error: Link \"%s\"'s origin has no rpy attribute\n", name_.c_str());
+ return false;
+ }
+ if (xyz.size() != 3)
+ {
+ fprintf(stderr, "Error: Link \"%s\"'s xyz origin is malformed\n", name_.c_str());
+ return false;
+ }
+ if (rpy.size() != 3)
+ {
+ fprintf(stderr, "Error: Link \"%s\"'s rpy origin is malformed\n", name_.c_str());
+ return false;
+ }
+ frame_ = KDL::Frame(KDL::Rotation::RPY(rpy[0], rpy[1], rpy[2]), KDL::Vector(xyz[0], xyz[1], xyz[2]));
+
// TODO: parse inertial info
// TODO: maybe parse collision info
Modified: pkg/trunk/mechanism/mechanism_model/src/robot.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/robot.cpp 2008-08-26 20:39:41 UTC (rev 3636)
+++ pkg/trunk/mechanism/mechanism_model/src/robot.cpp 2008-08-26 20:52:36 UTC (rev 3637)
@@ -38,6 +38,7 @@
bool Robot::initXml(TiXmlElement *root)
{
+ assert(hw_);
TiXmlElement *xit = NULL;
// Constructs the joints.
@@ -81,18 +82,16 @@
}
printLinkTree();
-#if 0
// Constructs the kinematic chains.
for (xit = root->FirstChildElement("chain"); xit;
xit = xit->NextSiblingElement("chain"))
{
- kinematics::Chain *c = new kinematics::Chain;
+ Chain *c = new Chain;
if (c->initXml(xit, this))
chains_.push_back(c);
else
delete c;
}
-#endif
return true;
}
@@ -117,6 +116,7 @@
Actuator* Robot::getActuator(const std::string &name)
{
+ assert(hw_);
// Yeah, it's a linear search. Deal with it.
return findByName(hw_->actuators_, name);
}
Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/include/urdf/parser.h
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/include/urdf/parser.h 2008-08-26 20:39:41 UTC (rev 3636)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/include/urdf/parser.h 2008-08-26 20:52:36 UTC (rev 3637)
@@ -34,6 +34,7 @@
#define URDF_FUNCTIONAL_PARSER_H
#include "tinyxml/tinyxml.h"
+#include "string_utils/string_utils.h"
namespace urdf {
@@ -42,6 +43,8 @@
TRANSMISSIONS = 2
};
+bool queryVectorAttribute(TiXmlElement *el, const char *name, std::vector<double> *value);
+
// Destructively modifies the document, replacing references to consts
// and blocks with the actual values.
bool normalizeXml(TiXmlElement *elt);
Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/src/parser.cpp
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/src/parser.cpp 2008-08-26 20:39:41 UTC (rev 3636)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/src/parser.cpp 2008-08-26 20:52:36 UTC (rev 3637)
@@ -32,6 +32,7 @@
*/
#include "urdf/parser.h"
#include <map>
+#include <vector>
#include <cmath>
#include <sstream>
#include "tinyxml/tinyxml.h"
@@ -42,6 +43,21 @@
namespace urdf {
+bool queryVectorAttribute(TiXmlElement *el, const char *name, std::vector<double> *value)
+{
+ value->clear();
+ const char *s = el->Attribute(name);
+ if (!s)
+ return false;
+
+ std::vector<std::string> pieces;
+ string_utils::split(s, pieces);
+ for (unsigned int i = 0; i < pieces.size(); ++i)
+ value->push_back(atof(pieces[i].c_str()));
+
+ return true;
+}
+
// Pulls the const and const_block elements out of the XML file.
class ConstsAndBlocks : public TiXmlVisitor
{
@@ -52,7 +68,7 @@
deleteValues(&blocks);
}
- virtual bool VisitEnter(const TiXmlElement &elt, const TiXmlAttribute *firstAttribute)
+ virtual bool VisitEnter(const TiXmlElement &elt, const TiXmlAttribute *)
{
if (elt.Value() == std::string("const"))
{
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
