12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added the config and units for a robot control system example that keeps a position and velocity model of a robot's motion.

release/4.3a0
Alex Cunningham 2009-11-26 05:52:17 +00:00
parent da36d07b93
commit 2a2e11d05d
8 changed files with 525 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
.cproject
View File

@ -543,6 +543,22 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testControlConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testControlConfig.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testControlPoint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testControlPoint.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildTarget>install</buildTarget> <buildTarget>install</buildTarget>

118
cpp/ControlConfig.cpp Normal file
View File

@ -0,0 +1,118 @@
/**
* @file ControlConfig.cpp
* @brief Implementation of ControlConfig
* @author Alex Cunningham
*/
#include <iostream>
#include <sstream>
#include <boost/tuple/tuple.hpp>
#include <boost/foreach.hpp>
#include "ControlConfig.h"
using namespace std;
using namespace gtsam;
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
// convert to strings
template<typename T>
string toStr(const T& t) {
ostringstream oss;
oss << t;
return oss.str();
}
/* *************************************************************** */
void ControlConfig::print(const std::string& name) const {
cout << "Config: " << name << endl;
string agent; path p;
FOREACH_PAIR(agent, p, paths_) {
cout << "Agent: " << agent << "\n";
int i = 0;
BOOST_FOREACH(ControlPoint pt, p) {
ostringstream oss;
oss << "Point: " << i++;
pt.print(oss.str());
}
cout << endl;
}
}
/* *************************************************************** */
bool ControlConfig::equals(const ControlConfig& expected, double tol) const {
if (paths_.size() != expected.paths_.size()) return false;
string j; path pa;
FOREACH_PAIR(j, pa, paths_) {
if (!expected.involvesAgent(j))
return false;
path pb = expected.getPath(j);
if (pa.size() != pb.size())
return false;
for (int i=0; i<pa.size(); ++i)
if (!pa.at(i).equals(pb.at(i), tol))
return false;
}
return true;
}
/* *************************************************************** */
void ControlConfig::addAgent(const std::string& name) {
if (paths_.find(name) == paths_.end()) {
path p;
paths_.insert(make_pair(name, p));
} else {
throw invalid_argument("Attempting to add already existing agent");
}
}
/* *************************************************************** */
void ControlConfig::addPoint(const std::string& name, const ControlPoint& state) {
if (paths_.find(name) != paths_.end()) {
path &p = paths_[name];
p.push_back(state);
} else {
throw invalid_argument("Attempting to add point without existing agent");
}
}
/* *************************************************************** */
ControlConfig::path ControlConfig::getPath(const std::string& agentID) const {
const_iterator it = paths_.find(agentID);
if (it != paths_.end()) {
return it->second;
} else {
throw invalid_argument("Attempting to access path that does not exist");
}
}
/* *************************************************************** */
bool ControlConfig::involvesAgent(const std::string& agentID) const {
return paths_.find(agentID) != paths_.end();
}
/* *************************************************************** */
void ControlConfig::clearAgent(const std::string& agentID) {
const_iterator it = paths_.find(agentID);
if (it != paths_.end()) {
path &p = paths_[agentID];
p.clear();
} else {
throw invalid_argument("Attempting to clear agent that is not present");
}
}
/* *************************************************************** */
ControlConfig ControlConfig::exmap(const VectorConfig & delta) const {
ControlConfig newConfig; string agent; path p;
FOREACH_PAIR(agent, p, paths_) {
newConfig.addAgent(agent);
for (size_t i=0; i<p.size(); ++i) {
string key = agent + "_" + toStr(i);
ControlPoint newPt = p.at(i).exmap(delta[key]);
newConfig.addPoint(agent, newPt);
}
}
return newConfig;
}

81
cpp/ControlConfig.h Normal file
View File

@ -0,0 +1,81 @@
/**
* @file ControlConfig.h
* @brief Class to describe a configuration of 2D agents that use PV models
* @author Alex Cunningham
*/
#pragma once
#include <map>
#include <vector>
#include "Testable.h"
#include "ControlPoint.h"
#include "VectorConfig.h"
namespace gtsam {
/**
* Class for configs of 2D agent motion models that make up trajectories
* Provides a map of labeled robot poses, and means to access groups, such
* as the trajectory of a particular robot or obstacle.
*/
class ControlConfig : public Testable<ControlConfig> {
public:
/** an individual path object for an agent */
typedef std::vector<ControlPoint> path;
typedef std::map<std::string, path>::const_iterator const_iterator;
private:
/** main storage for points */
std::map<std::string, path> paths_;
public:
/** Basic Default Constructors */
ControlConfig() {}
ControlConfig(const ControlConfig& cfg) : paths_(cfg.paths_) {}
/** Default destructor */
virtual ~ControlConfig() {}
/** Standard print function with optional label */
virtual void print(const std::string& name="") const;
/** Equality up to a tolerance */
virtual bool equals(const ControlConfig& expected, double tol=1e-9) const;
/** Add a delta configuration to the config */
ControlConfig exmap(const VectorConfig & delta) const;
/** number of agents */
size_t size() const { return paths_.size(); }
/**
* Adds an agent to the config
* @param name is the name of the agent used for lookup
*/
void addAgent(const std::string& name);
/**
* Adds a point to a robot's trajectory,
* note that ordering is handled internally
* @param name is the name of the robot
* @param state is the ControlPoint to add
*/
void addPoint(const std::string& name, const ControlPoint& state);
/**
* returns the path of a particular robot
*/
path getPath(const std::string& agentID) const;
/**
* Returns true if agent is in the config
*/
bool involvesAgent(const std::string& agentID) const;
// clearing
void clear() { paths_.clear(); }
void clearAgent(const std::string& agentID);
};
} // \namespace gtsam

48
cpp/ControlPoint.cpp Normal file
View File

@ -0,0 +1,48 @@
/**
* @file ControlPoint.cpp
* @brief Implementation of ControlPoint
* @author Alex Cunningham
*/
#include <iostream>
#include "ControlPoint.h"
using namespace std;
using namespace gtsam;
ControlPoint::ControlPoint()
: time_(0.0)
{ // Note that default pose2 constructors are at (0,0,0)
}
ControlPoint::ControlPoint(const Pose2& pos, const Pose2& vel, double time)
: pos_(pos), vel_(vel), time_(time)
{
}
ControlPoint::ControlPoint(double posx, double posy, double posr,
double velx, double vely, double velr, double time)
: pos_(posx, posy, posr), vel_(velx, vely, velr), time_(time)
{
}
void ControlPoint::print(const std::string& name) const {
cout << "ControlPoint: " << name << " at time = " << time_ << endl;
pos_.print("Position");
vel_.print("Velocity");
}
bool ControlPoint::equals(const ControlPoint& pt, double tol) const {
bool time_equal = abs(time_-pt.time()) < tol;
return time_equal && pos_.equals(pt.pos()) && vel_.equals(pt.vel());
}
ControlPoint ControlPoint::exmap(const Vector& delta) {
//TODO: bound the angle for position to -pi < theta < pi
Pose2 newPos(pos_.x()+delta(0), pos_.y()+delta(1), pos_.theta()+delta(2));
Pose2 newVel(vel_.x()+delta(3), vel_.y()+delta(4), vel_.theta()+delta(5));
double newTime = time_ + delta(6);
return ControlPoint(newPos, newVel, newTime);
}

70
cpp/ControlPoint.h Normal file
View File

@ -0,0 +1,70 @@
/*
* @file ControlPoint.h
* @brief Class with a point in a Position-Velocity model at a given time for 2D robots
* @author Alex Cunningham
*/
#pragma once
#include "Pose2.h"
#include "Testable.h"
namespace gtsam {
/**
* This class stores a single point in time using a model
* with position and velocity, as well as a time stamp, and
* is designed for use with robot control applications.
*/
class ControlPoint : public Testable<ControlPoint> {
private:
// position model
Pose2 pos_;
// velocity model
Pose2 vel_;
// timestamp for this observation
double time_;
public:
/** default contructor: stationary point at origin at zero time*/
ControlPoint();
/** full constructor */
ControlPoint(const Pose2& pos, const Pose2& vel, double time);
/** manual constructor - specify each Pose2 in full */
ControlPoint(double posx, double posy, double posr,
double velx, double vely, double velr, double time);
/** default destructor */
virtual ~ControlPoint() {}
/** Standard print function with optional label */
virtual void print(const std::string& name="") const;
/** Equality up to a tolerance */
virtual bool equals(const ControlPoint& expected, double tol=1e-9) const;
/* Access functions */
Pose2 pos() const { return pos_; }
Pose2 vel() const { return vel_; }
double time() const { return time_; }
/**
* Exmap function to add a delta configuration to the point
* NOTE: in handling rotation, the position will have its
* range bounded to -pi < r < pi, but the velocity
* can be larger than 2pi, as this would represent that
* the angular velocity will do more than a full rotation
* in a time step.
*/
ControlPoint exmap(const Vector& delta);
};
// comparison
}

9
cpp/Makefile.am
View File

@ -160,6 +160,15 @@ testSimulated3D_SOURCES = testSimulated3D.cpp
testSimulated3D_LDADD = libgtsam.la testSimulated3D_LDADD = libgtsam.la
check_PROGRAMS += testSimulated3D check_PROGRAMS += testSimulated3D
# Robot Control example system
sources += ControlConfig.cpp ControlPoint.cpp
#headers += ControlConfig.h ControlPoint.h
check_PROGRAMS += testControlConfig testControlPoint
testControlConfig_SOURCES = testControlConfig.cpp
testControlConfig_LDADD = libgtsam.la
testControlPoint_SOURCES = testControlPoint.cpp
testControlPoint_LDADD = libgtsam.la
# Cameras # Cameras
sources += CalibratedCamera.cpp SimpleCamera.cpp sources += CalibratedCamera.cpp SimpleCamera.cpp
check_PROGRAMS += testCalibratedCamera testSimpleCamera check_PROGRAMS += testCalibratedCamera testSimpleCamera

111
cpp/testControlConfig.cpp Normal file
View File

@ -0,0 +1,111 @@
/**
* @file testControlConfig.cpp
* @brief Test for configuration using 2D control inputs on a PV model
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <ControlConfig.h>
#include <ControlPoint.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST( ControlConfig, basic ) {
// create a config
ControlConfig config;
// add a robot to the config
string r1 = "R1", r2 = "R2";
config.addAgent(r1);
config.addAgent(r2);
// add some states for each of the robots
ControlPoint s1,
s2(Pose2(1.0, 1.0, 1.0), Pose2(), 2.0),
s3(Pose2(1.0, 1.0, 1.0), Pose2(), 3.0);
config.addPoint(r1, s1);
config.addPoint(r1, s2);
config.addPoint(r1, s3);
// get the path back out again
ControlConfig::path act1 = config.getPath(r1);
CHECK(act1.size() == 3);
CHECK(assert_equal(act1.at(0), s1));
CHECK(assert_equal(act1.at(1), s2));
CHECK(assert_equal(act1.at(2), s3));
// check size
CHECK(config.size() == 2);
}
/* ************************************************************************* */
TEST ( ControlConfig, equals ) {
ControlConfig cfg1, cfg2, cfg3;
cfg1.addAgent("r1");
cfg2.addAgent("r1");
cfg3.addAgent("r2");
CHECK(assert_equal(cfg1, cfg2));
CHECK(!cfg1.equals(cfg3));
ControlPoint s1, s2(Pose2(1.0, 1.0, 1.0), Pose2(), 2.0);
cfg1.addPoint("r1", s1);
cfg2.addPoint("r1", s2);
CHECK(!cfg1.equals(cfg2));
}
/* ************************************************************************* */
TEST ( ControlConfig, exmap ) {
// create a config with two agents and some trajectories
ControlConfig config;
ControlPoint s1,
s2(Pose2(1.0, 1.0, 1.0), Pose2(), 1.0),
s3(Pose2(2.0, 2.0, 2.0), Pose2(0.1, 0.2, 0.3), 2.0),
s4(Pose2(1.0, 2.0, 1.0), Pose2(), 0.0),
s5(Pose2(3.0, 4.0, 3.0), Pose2(0.4, 0.5, 0.6), 1.5);
config.addAgent("r1");
config.addPoint("r1", s1);
config.addPoint("r1", s2);
config.addPoint("r1", s3);
config.addAgent("r2");
config.addPoint("r2", s4);
config.addPoint("r2", s5);
// create a delta config
VectorConfig delta;
Vector d1 = repeat(7, 0.1);
Vector d2 = repeat(7, 0.2);
Vector d3 = repeat(7, 0.3);
Vector dother = repeat(7, 100.0);
delta.insert("r1_0", d1);
delta.insert("r1_1", d2);
delta.insert("r1_2", d3);
delta.insert("r2_0", d1);
delta.insert("r2_1", d2);
delta.insert("penguin", dother);
ControlConfig actual = config.exmap(delta);
// Verify
ControlConfig expected;
expected.addAgent("r1");
expected.addPoint("r1", s1.exmap(d1));
expected.addPoint("r1", s2.exmap(d2));
expected.addPoint("r1", s3.exmap(d3));
expected.addAgent("r2");
expected.addPoint("r2", s4.exmap(d1));
expected.addPoint("r2", s5.exmap(d2));
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

72
cpp/testControlPoint.cpp Normal file
View File

@ -0,0 +1,72 @@
/**
* @file testControlPoint.cpp
* @brief A single point in time on a trajectory
* @author Alex Cunningham
*/
#include <set>
#include <vector>
#include <boost/foreach.hpp>
#include <CppUnitLite/TestHarness.h>
#include <ControlPoint.h>
using namespace std;
using namespace gtsam;
TEST ( ControlPoint, constructors ) {
// make a default point, stationary at time zero at the origin
ControlPoint pt1;
CHECK(assert_equal(pt1.pos(), Pose2()));
CHECK(assert_equal(pt1.vel(), Pose2()));
CHECK(pt1.time() < 1e-9); // check for zero time
// make a point in same place to test constructors
ControlPoint pt2(Pose2(), Pose2(), 0.0);
CHECK(assert_equal(pt2.pos(), Pose2()));
CHECK(assert_equal(pt2.vel(), Pose2()));
CHECK(pt2.time() < 1e-9); // check for zero time
// check equality
CHECK(assert_equal(pt2, pt1));
// make a specific point
Pose2 pos(1.0, 2.0, 3.0);
Pose2 vel(0.1, 0.2, 0.3);
double time = 0.5;
ControlPoint pt3(pos, vel, time);
CHECK(assert_equal(pt3.pos(), pos));
CHECK(assert_equal(pt3.vel(), vel));
CHECK(fabs(pt3.time()-time) < 1e-9);
// use manual constructor
double posx=1.0, posy=2.0, posr=3.0;
double velx=0.1, vely=0.2, velr=0.3;
ControlPoint pt4(posx, posy, posr, velx, vely, velr, time);
CHECK(assert_equal(pt3, pt4));
}
TEST ( ControlPoint, exmap ) {
// add a delta to an existing point
Pose2 pos(1.0, 2.0, 3.0);
Pose2 vel(0.1, 0.2, 0.3);
double time = 0.5;
ControlPoint pt(pos, vel, time);
// ensure that zero vector doesn't change the point
ControlPoint act1 = pt.exmap(zero(7));
CHECK(assert_equal(pt, act1));
// add a real delta
Vector delta1 = Vector_(7, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7);
ControlPoint act2 = pt.exmap(delta1);
Pose2 pos_exp(1.1, 2.2, 3.3);
Pose2 vel_exp(0.5, 0.7, 0.9);
double time_exp = 1.2;
ControlPoint pt_exp(pos_exp, vel_exp, time_exp);
CHECK(assert_equal(act2, pt_exp));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */