Added the config and units for a robot control system example that keeps a position and velocity model of a robot's motion.
Alex Cunningham
parent
da36d07b93
commit
2a2e11d05d
16
.cproject
16
.cproject
|
|
@ -543,6 +543,22 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildTarget>install</buildTarget>
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
|
@ -160,6 +160,15 @@ testSimulated3D_SOURCES = testSimulated3D.cpp
|
|||
testSimulated3D_LDADD = libgtsam.la
|
||||
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
|
||||
sources += CalibratedCamera.cpp SimpleCamera.cpp
|
||||
check_PROGRAMS += testCalibratedCamera testSimpleCamera
|
||||
|
|
|
|||
|
|
@ -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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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); }
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue