136 lines
4.4 KiB
C++
136 lines
4.4 KiB
C++
/**
|
|
* @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); }
|
|
/* ************************************************************************* */
|