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

Added a graph implementation for multi-robot control systems. Currently can only really constrain the ends of a trajectory. 

Made a number of fixes and updates to the ControlConfig as well.
release/4.3a0
Alex Cunningham 2009-11-27 17:59:03 +00:00
parent f6ff04b75f
commit cf6474c99b
10 changed files with 554 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
.cproject
View File

@ -300,7 +300,6 @@
<buildTargets>
<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -308,7 +307,6 @@
</target>
<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -324,7 +322,6 @@
</target>
<target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimpleCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -340,6 +337,7 @@
</target>
<target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testVSLAMFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -347,7 +345,6 @@
</target>
<target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -355,6 +352,7 @@
</target>
<target name="testGaussianConditional.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianConditional.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -362,7 +360,6 @@
</target>
<target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -370,7 +367,6 @@
</target>
<target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -378,6 +374,7 @@
</target>
<target name="testNonlinearOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testNonlinearOptimizer.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -385,7 +382,6 @@
</target>
<target name="testGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -393,7 +389,6 @@
</target>
<target name="testGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -401,6 +396,7 @@
</target>
<target name="testNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testNonlinearFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -408,7 +404,6 @@
</target>
<target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -416,6 +411,7 @@
</target>
<target name="testVectorConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testVectorConfig.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -423,6 +419,7 @@
</target>
<target name="testPoint2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPoint2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -430,7 +427,6 @@
</target>
<target name="testNonlinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testNonlinearFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -438,7 +434,6 @@
</target>
<target name="timeGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>timeGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -446,7 +441,6 @@
</target>
<target name="timeGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>timeGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -454,7 +448,6 @@
</target>
<target name="testGaussianBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -462,6 +455,7 @@
</target>
<target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -469,7 +463,6 @@
</target>
<target name="testSymbolicBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -477,6 +470,7 @@
</target>
<target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -484,7 +478,6 @@
</target>
<target name="testVector.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -492,7 +485,6 @@
</target>
<target name="testMatrix.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testMatrix.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -500,6 +492,7 @@
</target>
<target name="testVSLAMGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testVSLAMGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -507,6 +500,7 @@
</target>
<target name="testNonlinearEquality.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testNonlinearEquality.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -514,7 +508,6 @@
</target>
<target name="testSQP.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSQP.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -522,6 +515,7 @@
</target>
<target name="testNonlinearConstraint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testNonlinearConstraint.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -529,7 +523,6 @@
</target>
<target name="testSQPOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSQPOptimizer.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -537,7 +530,6 @@
</target>
<target name="testVSLAMConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testVSLAMConfig.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -545,7 +537,6 @@
</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>
@ -553,14 +544,22 @@
</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="testControlGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testControlGraph.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>
<buildArguments/>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -568,6 +567,7 @@
</target>
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -575,6 +575,7 @@
</target>
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>

37
cpp/ControlConfig.cpp
View File

@ -62,16 +62,26 @@ 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) {
void ControlConfig::addPoint(const std::string& name, const ControlPoint& state, int index) {
if (index < -1 )
throw invalid_argument("Attempting to add point before start of trajectory");
if (paths_.find(name) != paths_.end()) {
path &p = paths_[name];
p.push_back(state);
if (index == -1) {
// just add the point to the back of the trajectory
p.push_back(state);
} else if (index < p.size()) {
// insert to existing point
p[index] = state;
} else {
// pad the trajectory to a particular size
p.resize(index+1);
p[index] = state;
}
} else {
throw invalid_argument("Attempting to add point without existing agent");
}
@ -116,3 +126,22 @@ ControlConfig ControlConfig::exmap(const VectorConfig & delta) const {
}
return newConfig;
}
/* *************************************************************** */
string ControlConfig::nameGen(const string& name, size_t num) {
return name + "_" + toStr(num);
}
/* *************************************************************** */
bool ControlConfig::compareConfigState(const std::string& key,
const ControlConfig& feasible, const ControlConfig& input) {
return feasible.get(key).equals(input.get(key));
}
/* *************************************************************** */
ControlPoint ControlConfig::get(const std::string& key) const {
size_t delim = key.find_first_of('_');
string agent = key.substr(0, delim);
int num = atoi(key.substr(delim+1).c_str());
return getPath(agent).at(num);
}

29
cpp/ControlConfig.h
View File

@ -21,6 +21,9 @@ namespace gtsam {
*/
class ControlConfig : public Testable<ControlConfig> {
public:
/** allow for shared pointers */
typedef boost::shared_ptr<ControlConfig> shared_config;
/** an individual path object for an agent */
typedef std::vector<ControlPoint> path;
typedef std::map<std::string, path>::const_iterator const_iterator;
@ -60,14 +63,19 @@ public:
* note that ordering is handled internally
* @param name is the name of the robot
* @param state is the ControlPoint to add
* @param index is the index in the trajectory to insert the point (defaults to
* pushing to the back of the trajectory)
*/
void addPoint(const std::string& name, const ControlPoint& state);
void addPoint(const std::string& name, const ControlPoint& state, int index=-1);
/**
* returns the path of a particular robot
*/
path getPath(const std::string& agentID) const;
/** get a vector in the configuration by key */
ControlPoint get(const std::string& key) const;
/**
* Returns true if agent is in the config
*/
@ -76,6 +84,25 @@ public:
// clearing
void clear() { paths_.clear(); }
void clearAgent(const std::string& agentID);
/**
* Generates a key for a key
* @param name is the name of the agent
* @param num is the sequence number of the robot
* @return a key in the form [name]_[num]
*/
static std::string nameGen(const std::string& name, size_t num);
/**
* Compares two values of a config
* Used for creating NonlinearEqualities
* @param key identifier for the constrained variable
* @param feasible defines the feasible set
* @param input is the config to compare
* @return true if the selected value in feasible equals the input config
*/
static bool compareConfigState(const std::string& key,
const ControlConfig& feasible, const ControlConfig& input);
};
} // \namespace gtsam

125
cpp/ControlGraph.cpp Normal file
View File

@ -0,0 +1,125 @@
/**
* @file ControlGraph.cpp
* @brief Implementation of a graph for solving robot control problems
* @author Alex Cunningham
*/
#include <iostream>
#include <boost/assign/list_inserter.hpp>
#include <boost/tuple/tuple.hpp>
#include "ControlGraph.h"
#include "NonlinearEquality.h"
#include "NonlinearFactorGraph-inl.h"
using namespace std;
using namespace gtsam;
using namespace boost::assign;
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
/* ************************************************************************* */
void ControlGraph::print(const std::string& name) const {
gtsam::NonlinearFactorGraph<ControlConfig>::print(name);
}
/* ************************************************************************* */
bool ControlGraph::equals(const ControlGraph& p, double tol) const {
if (&p == NULL) return false;
if (models_.size() != p.models_.size()) return false;
const_model_it it1 = models_.begin(), it2 = p.models_.begin();
for (; it1 != models_.end(), it2 != p.models_.end(); ++it1, ++it2) {
if (it1->first != it2->first) return false;
if (!it1->second.equals(it2->second)) return false;
}
return true;
}
/* ************************************************************************* */
void ControlGraph::addAgent(const std::string& name,
double maxVel, double maxAcc,
double maxRotVel, double maxRotAcc) {
addAgent(name,
ControlGraph::DynamicsModel(maxVel, maxAcc, maxRotVel, maxRotAcc));
}
/* ************************************************************************* */
void ControlGraph::addAgent(const std::string& name, const ControlGraph::DynamicsModel& model) {
insert(models_)(name, model);
}
/* ************************************************************************* */
ControlGraph::DynamicsModel ControlGraph::agentModel(const std::string& agent) const {
const_model_it it = models_.find(agent);
if (it != models_.end())
return it->second;
else
throw invalid_argument("Attempting to access invalid agent: " + agent);
}
/* ************************************************************************* */
void ControlGraph::addTrajectory(const std::string& name, size_t states) {
//TODO: Implement this function
// for each node to add
// add a temporal bounding constraint (first node is before second)
// add a path shortening factor (move points closer)
// add maximum velocity factor
// add velocity and acceleration clamping
}
/* ************************************************************************* */
void ControlGraph::fixAgentState(const std::string& name,
const ControlPoint& state, size_t state_num) {
// add a nonlinear equality constraint
typedef NonlinearEquality<ControlConfig> NLE;
feasible_.addAgent(name);
feasible_.addPoint(name, state, state_num);
boost::shared_ptr<NLE> constraint(new NLE(ControlConfig::nameGen(name, state_num),
feasible_, 7, ControlConfig::compareConfigState));
push_back(constraint);
}
/* ************************************************************************* */
set<string> ControlGraph::agents() const {
set<string> ret;
string key; ControlGraph::DynamicsModel m;
FOREACH_PAIR(key, m, models_) {
insert(ret)(key);
}
return ret;
}
/* ************************************************************************* */
// Implementation of DynamicsModel
/* ************************************************************************* */
ControlGraph::DynamicsModel::DynamicsModel()
: maxVel_(100.0), maxAcc_(100.0), maxRotVel_(100.0), maxRotAcc_(100.0)
{
}
/* ************************************************************************* */
ControlGraph::DynamicsModel::DynamicsModel(double maxVel, double maxAcc,
double maxRotVel, double maxRotAcc)
: maxVel_(maxVel), maxAcc_(maxAcc), maxRotVel_(maxRotVel), maxRotAcc_(maxRotAcc)
{
}
/* ************************************************************************* */
void ControlGraph::DynamicsModel::print(const std::string& name) const {
cout << "Dynamics Model: " << name << "\n"
<< " maxVel: " << maxVel_ << "\n"
<< " maxAcc: " << maxAcc_ << "\n"
<< " maxRotVel: " << maxRotVel_ << "\n"
<< " maxRotAcc: " << maxRotAcc_ << endl;
}
/* ************************************************************************* */
bool ControlGraph::DynamicsModel::equals(const DynamicsModel& m, double tol) const {
return maxVel_ == m.maxVel_ &&
maxAcc_ == m.maxAcc_ &&
maxRotVel_ == m.maxRotVel_ &&
maxRotAcc_ == m.maxRotAcc_;
}

133
cpp/ControlGraph.h Normal file
View File

@ -0,0 +1,133 @@
/*
* @file ControlGraph.h
* @brief Graph for robot control problems
* @author Alex Cunningham
*/
#pragma once
#include <map>
#include <set>
#include "NonlinearFactorGraph.h"
#include "FactorGraph-inl.h"
#include "ControlConfig.h"
namespace gtsam {
/**
* This graph manages the relationships between time-dependent
* points in robot trajectories. Each of these points manages its
* own time, so there will need to be constraints to ensure that
* the trajectories remain in the correct temporal ordering.
*/
class ControlGraph : public gtsam::NonlinearFactorGraph<ControlConfig>, Testable<ControlGraph> {
public:
/**
* Subclass to handle the model for individual agents
*/
class DynamicsModel : Testable<DynamicsModel>{
private:
double maxVel_;
double maxAcc_;
double maxRotVel_;
double maxRotAcc_;
public:
/** Constructor with unbounded limits */
DynamicsModel();
/** Constructor with initialization */
DynamicsModel(double maxVel, double maxAcc,
double maxRotVel, double maxRotAcc);
virtual ~DynamicsModel() {}
/** Standard print function with optional label */
void print(const std::string& name="") const;
/** Equality up to a tolerance */
bool equals(const DynamicsModel& expected, double tol=1e-9) const;
};
public:
// data typedefs
typedef std::map<std::string, DynamicsModel>::const_iterator const_model_it;
private:
/** models for the agents */
std::map<std::string, DynamicsModel> models_;
/** feasible set for constraints */
ControlConfig feasible_;
public:
/** Default constructor and destructor */
ControlGraph() {}
virtual ~ControlGraph() {}
/** Standard print function with optional label */
void print(const std::string& name="") const;
/** Equality up to a tolerance */
bool equals(const ControlGraph& expected, double tol=1e-9) const;
/**
* Adds an agent with parameters for the robot itself
* @param name is the name of the agent
* @param maxVel is the maximum translational velocity in distance/time
* @param maxAcc is the maximum translational acceleration in velocity/time
* @param maxRotVel is the maximum rotational velocity in radians/time
* @param maxRotAcc is the maximum rotational acceleration in anglar velocity/time
*/
void addAgent(const std::string& name,
double maxVel, double maxAcc,
double maxRotVel, double maxRotAcc);
/**
* Adds an agent with particular model
* @param name is the name of the agent
* @param model defines the characteristics of the robot
*/
void addAgent(const std::string& name, const DynamicsModel& model);
/** number of agents */
size_t nrAgents() const { return models_.size(); }
/** list of agents */
std::set<std::string> agents() const;
/**
* Gets the dynamics model for an agent
*/
DynamicsModel agentModel(const std::string& agent) const;
/**
* Creates a trajectory segment for a robot and adds it to the
* end of the existing path for given robot
* @param name is the name of the agent
* @param states is the number of additional states after the initial state
*/
void addTrajectory(const std::string& name, size_t states);
/**
* Fixes a particular state in a trajectory to a given point using
* a NonlinearEquality constraint. Use this for setting start and
* end points of trajectories.
* @param name is the name of the agent
* @param state is the value to fix the state to
* @param state_num is the number of the state to fix (defaults to first state)
*/
void fixAgentState(const std::string& name, const ControlPoint& state, size_t state_num=0);
/**
* Returns the feasible set for all of the constraints currently constructed
* @return config with constrained values
* NOTE: this will pad trajectories with default states
*/
ControlConfig feasible() const { return feasible_; }
};
} // \namespace gtsam

7
cpp/Makefile.am
View File

@ -161,13 +161,14 @@ 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
sources += ControlConfig.cpp ControlPoint.cpp ControlGraph.cpp
check_PROGRAMS += testControlConfig testControlPoint testControlGraph
testControlConfig_SOURCES = testControlConfig.cpp
testControlConfig_LDADD = libgtsam.la
testControlPoint_SOURCES = testControlPoint.cpp
testControlPoint_LDADD = libgtsam.la
testControlGraph_SOURCES = testControlGraph.cpp
testControlGraph_LDADD = libgtsam.la
# Cameras
sources += CalibratedCamera.cpp SimpleCamera.cpp

5
cpp/SQPOptimizer-inl.h
View File

@ -11,9 +11,12 @@
#include <boost/assign/std/map.hpp> // for insert
#include "GaussianFactorGraph.h"
#include "NonlinearFactorGraph.h"
#include "NonlinearFactorGraph-inl.h"
#include "SQPOptimizer.h"
// implementations
#include "NonlinearConstraint-inl.h"
#include "NonlinearFactorGraph-inl.h"
using namespace std;
using namespace boost::assign;

60
cpp/testControlConfig.cpp
View File

@ -41,6 +41,23 @@ TEST( ControlConfig, basic ) {
CHECK(config.size() == 2);
}
/* ************************************************************************* */
TEST ( ControlConfig, add_specific_points ) {
ControlConfig config;
ControlPoint s1,
s2(Pose2(1.0, 1.0, 1.0), Pose2(), 2.0),
s3(Pose2(2.0, 2.0, 1.0), Pose2(), 3.0);
config.addAgent("r1");
config.addPoint("r1", s1); // add at zero
config.addPoint("r1", s2, 5); // add at sequence 5
config.addPoint("r1", s3, 3); // add at sequence 3
CHECK(config.getPath("r1").size() == 6);
CHECK(assert_equal(config.getPath("r1").at(0), s1));
CHECK(assert_equal(config.getPath("r1").at(5), s2));
CHECK(assert_equal(config.getPath("r1").at(3), s3));
}
/* ************************************************************************* */
TEST ( ControlConfig, equals ) {
ControlConfig cfg1, cfg2, cfg3;
@ -68,7 +85,6 @@ TEST ( ControlConfig, exmap ) {
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);
@ -106,6 +122,48 @@ TEST ( ControlConfig, exmap ) {
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST ( ControlConfig, namegen ) {
string name = "r1";
int num = 5;
string actKey = ControlConfig::nameGen(name, num);
string expKey = "r1_5";
CHECK(actKey == expKey);
}
/* ************************************************************************* */
TEST ( ControlConfig, string_access ) {
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);
config.addAgent("r1");
config.addPoint("r1", s1);
config.addPoint("r1", s2);
config.addPoint("r1", s3);
CHECK(assert_equal(config.get("r1_0"), s1));
CHECK(assert_equal(config.get("r1_1"), s2));
CHECK(assert_equal(config.get("r1_2"), s3));
}
/* ************************************************************************* */
TEST ( ControlConfig, compare ) {
ControlConfig feasible, input;
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);
feasible.addAgent("r1");
feasible.addPoint("r1", s1);
feasible.addPoint("r1", s2);
input.addAgent("r1");
input.addPoint("r1", s1);
input.addPoint("r1", s3);
CHECK(ControlConfig::compareConfigState("r1_0", feasible, input));
CHECK(!ControlConfig::compareConfigState("r1_1", feasible, input));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

135
cpp/testControlGraph.cpp Normal file
View File

@ -0,0 +1,135 @@
/**
* @file testControlGraph.cpp
* @author Alex Cunningham
*/
#include <cmath>
#include <boost/assign/std/set.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/map.hpp> // for insert
#include <CppUnitLite/TestHarness.h>
#include "ControlGraph.h"
#include "ControlPoint.h"
#include "Ordering.h"
#include "SQPOptimizer.h"
#include "NonlinearEquality.h"
// implementations
#include "SQPOptimizer-inl.h"
using namespace std;
using namespace gtsam;
using namespace boost::assign;
typedef SQPOptimizer<ControlGraph, ControlConfig> COptimizer;
/* ************************************************************************* */
TEST (ControlGraph, add_agents) {
// create a graph with a robot with performance parameters
ControlGraph graph;
double maxVel=2.0, maxAcc=1.0, maxRotVel=3.0, maxRotAcc=1.0;
graph.addAgent("r1", maxVel, maxAcc, maxRotVel, maxRotAcc);
// read the robot information back out
CHECK(graph.nrAgents() == 1);
ControlGraph::DynamicsModel
actModel = graph.agentModel("r1"),
expModel(maxVel, maxAcc, maxRotVel, maxRotAcc);
CHECK(assert_equal(actModel, expModel));
// initialize a robot directly with a model
ControlGraph::DynamicsModel model2(1.0, 2.0, 3.0, 4.0), actModel2;
graph.addAgent("r2", model2);
CHECK(graph.nrAgents() == 2);
actModel2 = graph.agentModel("r2");
CHECK(assert_equal(actModel2, model2));
// get the names of the agents
set<string> actAgents = graph.agents(), expAgents;
expAgents += "r1", "r2";
CHECK(expAgents.size() == actAgents.size());
}
/* ************************************************************************* */
TEST (ControlGraph, equals) {
ControlGraph fg1, fg2;
CHECK(assert_equal(fg1, fg2));
fg1.addAgent("r1", 1.0, 1.0, 1.0, 1.0);
fg1.addAgent("r2", 1.0, 1.0, 1.0, 1.0);
CHECK(!fg1.equals(fg2));
fg2.addAgent("r1", 1.0, 1.0, 1.0, 1.0);
fg2.addAgent("r2", 1.0, 1.0, 1.0, 1.0);
CHECK(assert_equal(fg1, fg2));
}
/* ************************************************************************* */
TEST (ControlGraph, fix_constraints) {
// create a graph with a single robot
ControlGraph graph;
double maxVel, maxAcc;
maxVel = maxAcc = sqrt(2.0)+0.2;
graph.addAgent("r1", maxVel, maxAcc, 10.0, 10.0);
// constrain the ends
ControlPoint start,
end(Pose2(2.0, 2.0, 0.0), Pose2(), 2.0);
graph.fixAgentState("r1", start, 0);
graph.fixAgentState("r1", end, 2);
// extract the constraints
typedef NonlinearEquality<ControlConfig> NLE;
typedef NonlinearFactor<ControlConfig> NLF;
boost::shared_ptr<NLF> cStart = graph[0], cEnd = graph[1];
boost::shared_ptr<NLE> actStart = boost::shared_dynamic_cast<NLE>(cStart);
boost::shared_ptr<NLE> actEnd = boost::shared_dynamic_cast<NLE>(cEnd);
// fetch feasible set from graph
ControlConfig feasible = graph.feasible();
// create expected values
NLE expStart("r1_0", feasible, 7, ControlConfig::compareConfigState);
NLE expEnd("r1_2", feasible, 7, ControlConfig::compareConfigState);
CHECK(assert_equal(expStart, *actStart));
CHECK(assert_equal(expEnd, *actEnd));
}
/* ************************************************************************* */
TEST (ControlGraph, automated_graphgen_optimization) {
// create a graph with a robot with performance parameters
ControlGraph graph;
double maxVel=2.0, maxAcc=1.0, maxRotVel=3.0, maxRotAcc=1.0;
graph.addAgent("r1", maxVel, maxAcc, maxRotVel, maxRotAcc);
// fix initial states for the agents
graph.fixAgentState("r1", ControlPoint()); // default argument fixes start
// create a three-state trajectory (4 including the initial state)
graph.addTrajectory("r1", 3);
// fix the end of the trajectory
ControlPoint endPt(Pose2(5.0, 0.0, 0.0), Pose2(), 5.0);
graph.fixAgentState("r1", endPt, 3);
// create an initial config
ControlConfig::shared_config initConfig(new ControlConfig);
initConfig->addAgent("r1");
initConfig->addPoint("r1", ControlPoint());
initConfig->addPoint("r1", ControlPoint());
initConfig->addPoint("r1", ControlPoint());
initConfig->addPoint("r1", endPt);
// create an ordering
Ordering ordering;
ordering += "r1_0", "r1_1", "r1_2", "r1_3";
// // create an optimizer
// COptimizer optimizer(graph, ordering, initConfig);
//
// // do an iteration
// COptimizer oneIteration = optimizer.iterate(COptimizer::FULL);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

15
cpp/testNonlinearEquality.cpp
View File

@ -21,6 +21,7 @@ bool vector_compare(const std::string& key,
return equal_with_abs_tol(lin, feas, 1e-5);
}
/* ************************************************************************* */
TEST ( NonlinearEquality, linearization ) {
string key = "x";
Vector value = Vector_(2, 1.0, 2.0);
@ -37,6 +38,7 @@ TEST ( NonlinearEquality, linearization ) {
CHECK(assert_equal(*actualLF, expLF));
}
/* ************************************************************************* */
TEST ( NonlinearEquality, linearization_fail ) {
string key = "x";
Vector value = Vector_(2, 1.0, 2.0);
@ -57,6 +59,7 @@ TEST ( NonlinearEquality, linearization_fail ) {
}
}
/* ************************************************************************* */
TEST ( NonlinearEquality, error ) {
string key = "x";
Vector value = Vector_(2, 1.0, 2.0);
@ -76,24 +79,28 @@ TEST ( NonlinearEquality, error ) {
CHECK(assert_equal(actual, repeat(2, 1.0/0.0)));
}
/* ************************************************************************* */
TEST ( NonlinearEquality, equals ) {
string key1 = "x";
Vector value1 = Vector_(2, 1.0, 2.0);
Vector value2 = Vector_(2, 3.0, 4.0);
VectorConfig feasible1, feasible2;
VectorConfig feasible1, feasible2, feasible3;
feasible1.insert(key1, value1);
feasible2.insert(key1, value2);
feasible3.insert(key1, value1);
feasible3.insert("y", value2);
// create some constraints to compare
shared_nle nle1(new NonlinearEquality<VectorConfig>(key1, feasible1, 2, *vector_compare));
shared_nle nle2(new NonlinearEquality<VectorConfig>(key1, feasible1, 2, *vector_compare));
shared_nle nle3(new NonlinearEquality<VectorConfig>(key1, feasible2, 2, *vector_compare));
shared_nle nle4(new NonlinearEquality<VectorConfig>(key1, feasible3, 2, *vector_compare));
// verify
CHECK(nle1->equals(*nle2)); // basic equality = true
CHECK(nle2->equals(*nle1)); // test symmetry of equals()
CHECK(nle1->equals(*nle2)); // basic equality = true
CHECK(nle2->equals(*nle1)); // test symmetry of equals()
CHECK(!nle1->equals(*nle3)); // test config
CHECK(nle4->equals(*nle1)); // test the full feasible set isn't getting compared
}