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_SOURCES = $(example) testPose3Factor.cpp
|
||||||
testPose3Factor_LDADD = libgtsam.la
|
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
|
# Cameras
|
||||||
sources += CalibratedCamera.cpp SimpleCamera.cpp
|
sources += CalibratedCamera.cpp SimpleCamera.cpp
|
||||||
check_PROGRAMS += testCalibratedCamera testSimpleCamera
|
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