diff --git a/cpp/ControlConfig.cpp b/cpp/ControlConfig.cpp deleted file mode 100644 index 23fafc175..000000000 --- a/cpp/ControlConfig.cpp +++ /dev/null @@ -1,147 +0,0 @@ -/** - * @file ControlConfig.cpp - * @brief Implementation of ControlConfig - * @author Alex Cunningham - */ - -#include -#include -#include -#include -#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 -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; isecond; - } 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 -#include -#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 { -public: - /** allow for shared pointers */ - typedef boost::shared_ptr shared_config; - - /** an individual path object for an agent */ - typedef std::vector path; - typedef std::map::const_iterator const_iterator; - -private: - /** main storage for points */ - std::map 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 - * @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, 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 - */ - bool involvesAgent(const std::string& agentID) const; - - // 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 - diff --git a/cpp/ControlGraph.cpp b/cpp/ControlGraph.cpp deleted file mode 100644 index 13291f810..000000000 --- a/cpp/ControlGraph.cpp +++ /dev/null @@ -1,125 +0,0 @@ -/** - * @file ControlGraph.cpp - * @brief Implementation of a graph for solving robot control problems - * @author Alex Cunningham - */ - -#include -#include -#include -#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::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 NLE; - feasible_.addAgent(name); - feasible_.addPoint(name, state, state_num); - boost::shared_ptr constraint(new NLE(ControlConfig::nameGen(name, state_num), - feasible_, 7, ControlConfig::compareConfigState)); - push_back(constraint); -} - -/* ************************************************************************* */ -set ControlGraph::agents() const { - set 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_; -} diff --git a/cpp/ControlGraph.h b/cpp/ControlGraph.h deleted file mode 100644 index 718ee1ee8..000000000 --- a/cpp/ControlGraph.h +++ /dev/null @@ -1,133 +0,0 @@ -/* - * @file ControlGraph.h - * @brief Graph for robot control problems - * @author Alex Cunningham - */ - -#pragma once - -#include -#include -#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, Testable { -public: - /** - * Subclass to handle the model for individual agents - */ - class DynamicsModel : Testable{ - 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::const_iterator const_model_it; - -private: - /** models for the agents */ - std::map 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 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 - - diff --git a/cpp/ControlPoint.cpp b/cpp/ControlPoint.cpp deleted file mode 100644 index ded9a5bb8..000000000 --- a/cpp/ControlPoint.cpp +++ /dev/null @@ -1,48 +0,0 @@ -/** - * @file ControlPoint.cpp - * @brief Implementation of ControlPoint - * @author Alex Cunningham - */ - -#include -#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); -} - - diff --git a/cpp/ControlPoint.h b/cpp/ControlPoint.h deleted file mode 100644 index 0708edd82..000000000 --- a/cpp/ControlPoint.h +++ /dev/null @@ -1,70 +0,0 @@ -/* - * @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 { -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 - -} - - - diff --git a/cpp/Makefile.am b/cpp/Makefile.am index e106f269c..5f84895ec 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -202,16 +202,6 @@ testPose2Graph_LDADD = libgtsam.la testPose3Factor_SOURCES = $(example) testPose3Factor.cpp testPose3Factor_LDADD = libgtsam.la -# Robot Control example system -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 check_PROGRAMS += testCalibratedCamera testSimpleCamera diff --git a/cpp/testControlConfig.cpp b/cpp/testControlConfig.cpp deleted file mode 100644 index e4006234e..000000000 --- a/cpp/testControlConfig.cpp +++ /dev/null @@ -1,169 +0,0 @@ -/** - * @file testControlConfig.cpp - * @brief Test for configuration using 2D control inputs on a PV model - * @author Alex Cunningham - */ - -#include -#include -#include - -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, 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; - 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)); -} - -/* ************************************************************************* */ -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); } -/* ************************************************************************* */ diff --git a/cpp/testControlGraph.cpp b/cpp/testControlGraph.cpp deleted file mode 100644 index 4aca31aba..000000000 --- a/cpp/testControlGraph.cpp +++ /dev/null @@ -1,135 +0,0 @@ -/** - * @file testControlGraph.cpp - * @author Alex Cunningham - */ - -#include -#include -#include // for operator += -#include // for insert -#include -#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 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 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 NLE; - typedef NonlinearFactor NLF; - boost::shared_ptr cStart = graph[0], cEnd = graph[1]; - boost::shared_ptr actStart = boost::shared_dynamic_cast(cStart); - boost::shared_ptr actEnd = boost::shared_dynamic_cast(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); } -/* ************************************************************************* */ diff --git a/cpp/testControlPoint.cpp b/cpp/testControlPoint.cpp deleted file mode 100644 index 00724b34b..000000000 --- a/cpp/testControlPoint.cpp +++ /dev/null @@ -1,72 +0,0 @@ -/** - * @file testControlPoint.cpp - * @brief A single point in time on a trajectory - * @author Alex Cunningham - */ - -#include -#include -#include -#include -#include - -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); } -/* ************************************************************************* */