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