Moved control-related components to separate library
Alex Cunningham
parent
b20ed42134
commit
20c6f29823
|
|
@ -1,147 +0,0 @@
|
|||
/**
|
||||
* @file ControlConfig.cpp
|
||||
* @brief Implementation of ControlConfig
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include "ControlConfig.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// trick from some reading group
|
||||
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
|
||||
|
||||
// convert to strings
|
||||
template<typename T>
|
||||
string toStr(const T& t) {
|
||||
ostringstream oss;
|
||||
oss << t;
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
/* *************************************************************** */
|
||||
void ControlConfig::print(const std::string& name) const {
|
||||
cout << "Config: " << name << endl;
|
||||
string agent; path p;
|
||||
FOREACH_PAIR(agent, p, paths_) {
|
||||
cout << "Agent: " << agent << "\n";
|
||||
int i = 0;
|
||||
BOOST_FOREACH(ControlPoint pt, p) {
|
||||
ostringstream oss;
|
||||
oss << "Point: " << i++;
|
||||
pt.print(oss.str());
|
||||
}
|
||||
cout << endl;
|
||||
}
|
||||
}
|
||||
|
||||
/* *************************************************************** */
|
||||
bool ControlConfig::equals(const ControlConfig& expected, double tol) const {
|
||||
if (paths_.size() != expected.paths_.size()) return false;
|
||||
string j; path pa;
|
||||
FOREACH_PAIR(j, pa, paths_) {
|
||||
if (!expected.involvesAgent(j))
|
||||
return false;
|
||||
path pb = expected.getPath(j);
|
||||
if (pa.size() != pb.size())
|
||||
return false;
|
||||
for (int i=0; i<pa.size(); ++i)
|
||||
if (!pa.at(i).equals(pb.at(i), tol))
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/* *************************************************************** */
|
||||
void ControlConfig::addAgent(const std::string& name) {
|
||||
if (paths_.find(name) == paths_.end()) {
|
||||
path p;
|
||||
paths_.insert(make_pair(name, p));
|
||||
}
|
||||
}
|
||||
|
||||
/* *************************************************************** */
|
||||
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];
|
||||
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");
|
||||
}
|
||||
}
|
||||
|
||||
/* *************************************************************** */
|
||||
ControlConfig::path ControlConfig::getPath(const std::string& agentID) const {
|
||||
const_iterator it = paths_.find(agentID);
|
||||
if (it != paths_.end()) {
|
||||
return it->second;
|
||||
} else {
|
||||
throw invalid_argument("Attempting to access path that does not exist");
|
||||
}
|
||||
}
|
||||
|
||||
/* *************************************************************** */
|
||||
bool ControlConfig::involvesAgent(const std::string& agentID) const {
|
||||
return paths_.find(agentID) != paths_.end();
|
||||
}
|
||||
|
||||
/* *************************************************************** */
|
||||
void ControlConfig::clearAgent(const std::string& agentID) {
|
||||
const_iterator it = paths_.find(agentID);
|
||||
if (it != paths_.end()) {
|
||||
path &p = paths_[agentID];
|
||||
p.clear();
|
||||
} else {
|
||||
throw invalid_argument("Attempting to clear agent that is not present");
|
||||
}
|
||||
}
|
||||
|
||||
/* *************************************************************** */
|
||||
ControlConfig ControlConfig::exmap(const VectorConfig & delta) const {
|
||||
ControlConfig newConfig; string agent; path p;
|
||||
FOREACH_PAIR(agent, p, paths_) {
|
||||
newConfig.addAgent(agent);
|
||||
for (size_t i=0; i<p.size(); ++i) {
|
||||
string key = agent + "_" + toStr(i);
|
||||
ControlPoint newPt = p.at(i).exmap(delta[key]);
|
||||
newConfig.addPoint(agent, newPt);
|
||||
}
|
||||
}
|
||||
return newConfig;
|
||||
}
|
||||
|
||||
/* *************************************************************** */
|
||||
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);
|
||||
}
|
||||
|
|
@ -1,108 +0,0 @@
|
|||
/**
|
||||
* @file ControlConfig.h
|
||||
* @brief Class to describe a configuration of 2D agents that use PV models
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <map>
|
||||
#include <vector>
|
||||
#include "Testable.h"
|
||||
#include "ControlPoint.h"
|
||||
#include "VectorConfig.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Class for configs of 2D agent motion models that make up trajectories
|
||||
* Provides a map of labeled robot poses, and means to access groups, such
|
||||
* as the trajectory of a particular robot or obstacle.
|
||||
*/
|
||||
class ControlConfig : public Testable<ControlConfig> {
|
||||
public:
|
||||
/** 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;
|
||||
|
||||
private:
|
||||
/** main storage for points */
|
||||
std::map<std::string, path> paths_;
|
||||
|
||||
public:
|
||||
/** Basic Default Constructors */
|
||||
ControlConfig() {}
|
||||
ControlConfig(const ControlConfig& cfg) : paths_(cfg.paths_) {}
|
||||
|
||||
/** Default destructor */
|
||||
virtual ~ControlConfig() {}
|
||||
|
||||
/** Standard print function with optional label */
|
||||
virtual void print(const std::string& name="") const;
|
||||
|
||||
/** Equality up to a tolerance */
|
||||
virtual bool equals(const ControlConfig& expected, double tol=1e-9) const;
|
||||
|
||||
/** Add a delta configuration to the config */
|
||||
ControlConfig exmap(const VectorConfig & delta) const;
|
||||
|
||||
/** number of agents */
|
||||
size_t size() const { return paths_.size(); }
|
||||
|
||||
/**
|
||||
* Adds an agent to the config
|
||||
* @param name is the name of the agent used for lookup
|
||||
*/
|
||||
void addAgent(const std::string& name);
|
||||
|
||||
/**
|
||||
* Adds a point to a robot's trajectory,
|
||||
* note that ordering is handled internally
|
||||
* @param name is the name of the robot
|
||||
* @param state is the ControlPoint to add
|
||||
* @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
|
||||
|
||||
|
|
@ -1,125 +0,0 @@
|
|||
/**
|
||||
* @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_;
|
||||
}
|
||||
|
|
@ -1,133 +0,0 @@
|
|||
/*
|
||||
* @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
|
||||
|
||||
|
||||
|
|
@ -1,48 +0,0 @@
|
|||
/**
|
||||
* @file ControlPoint.cpp
|
||||
* @brief Implementation of ControlPoint
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
#include "ControlPoint.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
ControlPoint::ControlPoint()
|
||||
: time_(0.0)
|
||||
{ // Note that default pose2 constructors are at (0,0,0)
|
||||
}
|
||||
|
||||
ControlPoint::ControlPoint(const Pose2& pos, const Pose2& vel, double time)
|
||||
: pos_(pos), vel_(vel), time_(time)
|
||||
{
|
||||
}
|
||||
|
||||
ControlPoint::ControlPoint(double posx, double posy, double posr,
|
||||
double velx, double vely, double velr, double time)
|
||||
: pos_(posx, posy, posr), vel_(velx, vely, velr), time_(time)
|
||||
{
|
||||
}
|
||||
|
||||
void ControlPoint::print(const std::string& name) const {
|
||||
cout << "ControlPoint: " << name << " at time = " << time_ << endl;
|
||||
pos_.print("Position");
|
||||
vel_.print("Velocity");
|
||||
}
|
||||
|
||||
bool ControlPoint::equals(const ControlPoint& pt, double tol) const {
|
||||
bool time_equal = abs(time_-pt.time()) < tol;
|
||||
return time_equal && pos_.equals(pt.pos()) && vel_.equals(pt.vel());
|
||||
}
|
||||
|
||||
ControlPoint ControlPoint::exmap(const Vector& delta) {
|
||||
//TODO: bound the angle for position to -pi < theta < pi
|
||||
Pose2 newPos(pos_.x()+delta(0), pos_.y()+delta(1), pos_.theta()+delta(2));
|
||||
Pose2 newVel(vel_.x()+delta(3), vel_.y()+delta(4), vel_.theta()+delta(5));
|
||||
double newTime = time_ + delta(6);
|
||||
return ControlPoint(newPos, newVel, newTime);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -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<ControlPoint> {
|
||||
private:
|
||||
// position model
|
||||
Pose2 pos_;
|
||||
|
||||
// velocity model
|
||||
Pose2 vel_;
|
||||
|
||||
// timestamp for this observation
|
||||
double time_;
|
||||
|
||||
public:
|
||||
/** default contructor: stationary point at origin at zero time*/
|
||||
ControlPoint();
|
||||
|
||||
/** full constructor */
|
||||
ControlPoint(const Pose2& pos, const Pose2& vel, double time);
|
||||
|
||||
/** manual constructor - specify each Pose2 in full */
|
||||
ControlPoint(double posx, double posy, double posr,
|
||||
double velx, double vely, double velr, double time);
|
||||
|
||||
/** default destructor */
|
||||
virtual ~ControlPoint() {}
|
||||
|
||||
/** Standard print function with optional label */
|
||||
virtual void print(const std::string& name="") const;
|
||||
|
||||
/** Equality up to a tolerance */
|
||||
virtual bool equals(const ControlPoint& expected, double tol=1e-9) const;
|
||||
|
||||
/* Access functions */
|
||||
Pose2 pos() const { return pos_; }
|
||||
Pose2 vel() const { return vel_; }
|
||||
double time() const { return time_; }
|
||||
|
||||
/**
|
||||
* Exmap function to add a delta configuration to the point
|
||||
* NOTE: in handling rotation, the position will have its
|
||||
* range bounded to -pi < r < pi, but the velocity
|
||||
* can be larger than 2pi, as this would represent that
|
||||
* the angular velocity will do more than a full rotation
|
||||
* in a time step.
|
||||
*/
|
||||
ControlPoint exmap(const Vector& delta);
|
||||
};
|
||||
// comparison
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -1,169 +0,0 @@
|
|||
/**
|
||||
* @file testControlConfig.cpp
|
||||
* @brief Test for configuration using 2D control inputs on a PV model
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <ControlConfig.h>
|
||||
#include <ControlPoint.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ControlConfig, basic ) {
|
||||
|
||||
// create a config
|
||||
ControlConfig config;
|
||||
|
||||
// add a robot to the config
|
||||
string r1 = "R1", r2 = "R2";
|
||||
config.addAgent(r1);
|
||||
config.addAgent(r2);
|
||||
|
||||
// add some states for each of the robots
|
||||
ControlPoint s1,
|
||||
s2(Pose2(1.0, 1.0, 1.0), Pose2(), 2.0),
|
||||
s3(Pose2(1.0, 1.0, 1.0), Pose2(), 3.0);
|
||||
config.addPoint(r1, s1);
|
||||
config.addPoint(r1, s2);
|
||||
config.addPoint(r1, s3);
|
||||
|
||||
// get the path back out again
|
||||
ControlConfig::path act1 = config.getPath(r1);
|
||||
CHECK(act1.size() == 3);
|
||||
CHECK(assert_equal(act1.at(0), s1));
|
||||
CHECK(assert_equal(act1.at(1), s2));
|
||||
CHECK(assert_equal(act1.at(2), s3));
|
||||
|
||||
// check size
|
||||
CHECK(config.size() == 2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST ( ControlConfig, 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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -1,135 +0,0 @@
|
|||
/**
|
||||
* @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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -1,72 +0,0 @@
|
|||
/**
|
||||
* @file testControlPoint.cpp
|
||||
* @brief A single point in time on a trajectory
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <set>
|
||||
#include <vector>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <ControlPoint.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
TEST ( ControlPoint, constructors ) {
|
||||
|
||||
// make a default point, stationary at time zero at the origin
|
||||
ControlPoint pt1;
|
||||
CHECK(assert_equal(pt1.pos(), Pose2()));
|
||||
CHECK(assert_equal(pt1.vel(), Pose2()));
|
||||
CHECK(pt1.time() < 1e-9); // check for zero time
|
||||
|
||||
// make a point in same place to test constructors
|
||||
ControlPoint pt2(Pose2(), Pose2(), 0.0);
|
||||
CHECK(assert_equal(pt2.pos(), Pose2()));
|
||||
CHECK(assert_equal(pt2.vel(), Pose2()));
|
||||
CHECK(pt2.time() < 1e-9); // check for zero time
|
||||
|
||||
// check equality
|
||||
CHECK(assert_equal(pt2, pt1));
|
||||
|
||||
// make a specific point
|
||||
Pose2 pos(1.0, 2.0, 3.0);
|
||||
Pose2 vel(0.1, 0.2, 0.3);
|
||||
double time = 0.5;
|
||||
ControlPoint pt3(pos, vel, time);
|
||||
CHECK(assert_equal(pt3.pos(), pos));
|
||||
CHECK(assert_equal(pt3.vel(), vel));
|
||||
CHECK(fabs(pt3.time()-time) < 1e-9);
|
||||
|
||||
// use manual constructor
|
||||
double posx=1.0, posy=2.0, posr=3.0;
|
||||
double velx=0.1, vely=0.2, velr=0.3;
|
||||
ControlPoint pt4(posx, posy, posr, velx, vely, velr, time);
|
||||
CHECK(assert_equal(pt3, pt4));
|
||||
}
|
||||
|
||||
TEST ( ControlPoint, exmap ) {
|
||||
// add a delta to an existing point
|
||||
Pose2 pos(1.0, 2.0, 3.0);
|
||||
Pose2 vel(0.1, 0.2, 0.3);
|
||||
double time = 0.5;
|
||||
ControlPoint pt(pos, vel, time);
|
||||
|
||||
// ensure that zero vector doesn't change the point
|
||||
ControlPoint act1 = pt.exmap(zero(7));
|
||||
CHECK(assert_equal(pt, act1));
|
||||
|
||||
// add a real delta
|
||||
Vector delta1 = Vector_(7, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7);
|
||||
ControlPoint act2 = pt.exmap(delta1);
|
||||
Pose2 pos_exp(1.1, 2.2, 3.3);
|
||||
Pose2 vel_exp(0.5, 0.7, 0.9);
|
||||
double time_exp = 1.2;
|
||||
ControlPoint pt_exp(pos_exp, vel_exp, time_exp);
|
||||
CHECK(assert_equal(act2, pt_exp));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue