diff --git a/.cproject b/.cproject index 85c3942c4..83a6da8a5 100644 --- a/.cproject +++ b/.cproject @@ -300,7 +300,6 @@ make - install true true @@ -308,7 +307,6 @@ make - check true true @@ -324,7 +322,6 @@ make - testSimpleCamera.run true true @@ -340,6 +337,7 @@ make + testVSLAMFactor.run true true @@ -347,7 +345,6 @@ make - testCalibratedCamera.run true true @@ -355,6 +352,7 @@ make + testGaussianConditional.run true true @@ -362,7 +360,6 @@ make - testPose2.run true true @@ -370,7 +367,6 @@ make - testRot3.run true true @@ -378,6 +374,7 @@ make + testNonlinearOptimizer.run true true @@ -385,7 +382,6 @@ make - testGaussianFactor.run true true @@ -393,7 +389,6 @@ make - testGaussianFactorGraph.run true true @@ -401,6 +396,7 @@ make + testNonlinearFactorGraph.run true true @@ -408,7 +404,6 @@ make - testPose3.run true true @@ -416,6 +411,7 @@ make + testVectorConfig.run true true @@ -423,6 +419,7 @@ make + testPoint2.run true true @@ -430,7 +427,6 @@ make - testNonlinearFactor.run true true @@ -438,7 +434,6 @@ make - timeGaussianFactor.run true true @@ -446,7 +441,6 @@ make - timeGaussianFactorGraph.run true true @@ -454,7 +448,6 @@ make - testGaussianBayesNet.run true true @@ -462,6 +455,7 @@ make + testBayesTree.run true false @@ -469,7 +463,6 @@ make - testSymbolicBayesNet.run true false @@ -477,6 +470,7 @@ make + testSymbolicFactorGraph.run true false @@ -484,7 +478,6 @@ make - testVector.run true true @@ -492,7 +485,6 @@ make - testMatrix.run true true @@ -500,6 +492,7 @@ make + testVSLAMGraph.run true true @@ -507,6 +500,7 @@ make + testNonlinearEquality.run true true @@ -514,7 +508,6 @@ make - testSQP.run true true @@ -522,6 +515,7 @@ make + testNonlinearConstraint.run true true @@ -529,7 +523,6 @@ make - testSQPOptimizer.run true true @@ -537,7 +530,6 @@ make - testVSLAMConfig.run true true @@ -545,7 +537,6 @@ make - testControlConfig.run true true @@ -553,14 +544,22 @@ make - testControlPoint.run true true true + +make + +testControlGraph.run +true +true +true + make + install true true @@ -568,6 +567,7 @@ make + clean true true @@ -575,6 +575,7 @@ make + check true true diff --git a/cpp/ControlConfig.cpp b/cpp/ControlConfig.cpp index 9b3a3d125..23fafc175 100644 --- a/cpp/ControlConfig.cpp +++ b/cpp/ControlConfig.cpp @@ -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); +} diff --git a/cpp/ControlConfig.h b/cpp/ControlConfig.h index 3a7fab025..31db9a9ba 100644 --- a/cpp/ControlConfig.h +++ b/cpp/ControlConfig.h @@ -21,6 +21,9 @@ namespace gtsam { */ 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; @@ -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 diff --git a/cpp/ControlGraph.cpp b/cpp/ControlGraph.cpp new file mode 100644 index 000000000..13291f810 --- /dev/null +++ b/cpp/ControlGraph.cpp @@ -0,0 +1,125 @@ +/** + * @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 new file mode 100644 index 000000000..718ee1ee8 --- /dev/null +++ b/cpp/ControlGraph.h @@ -0,0 +1,133 @@ +/* + * @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/Makefile.am b/cpp/Makefile.am index 2da91073f..f282a8d1c 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -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 diff --git a/cpp/SQPOptimizer-inl.h b/cpp/SQPOptimizer-inl.h index 78bf88399..77bfc77dc 100644 --- a/cpp/SQPOptimizer-inl.h +++ b/cpp/SQPOptimizer-inl.h @@ -11,9 +11,12 @@ #include // 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; diff --git a/cpp/testControlConfig.cpp b/cpp/testControlConfig.cpp index c96168012..e4006234e 100644 --- a/cpp/testControlConfig.cpp +++ b/cpp/testControlConfig.cpp @@ -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); } /* ************************************************************************* */ diff --git a/cpp/testControlGraph.cpp b/cpp/testControlGraph.cpp new file mode 100644 index 000000000..4aca31aba --- /dev/null +++ b/cpp/testControlGraph.cpp @@ -0,0 +1,135 @@ +/** + * @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/testNonlinearEquality.cpp b/cpp/testNonlinearEquality.cpp index 7f0a81bf2..ff2f58bd4 100644 --- a/cpp/testNonlinearEquality.cpp +++ b/cpp/testNonlinearEquality.cpp @@ -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(key1, feasible1, 2, *vector_compare)); shared_nle nle2(new NonlinearEquality(key1, feasible1, 2, *vector_compare)); shared_nle nle3(new NonlinearEquality(key1, feasible2, 2, *vector_compare)); + shared_nle nle4(new NonlinearEquality(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 }