/*!
* This constructor builds a non-reversible rate matrix
* for discrete characters.
*
* \brief Creates non-reversible rate matrix
* \param rate The irrev rates
* \param useEigen Use eigensystem (true) or Pade approximation (false)
*
*/
MbTransitionMatrix::MbTransitionMatrix(const std::vector<double> &rate, bool useEigen)
: c_ijk(0), cc_ijk(0), ceigenvalue(0), ceigValExp(0), eigens(0), eigenvalue(0), eigValExp(0),
isComplex(false), isOldComplex(false), isRev(false), numStates(0), oldC_ijk(0), oldEigenvalue(0),
oldCC_ijk(0), oldCEigenvalue(0), useEigens(useEigen), hasUniformizedMatrix(false) {
// Find number of rates
int nSt = (int) (floor(sqrt((double)rate.size()))) + 1;
// If the number of rates is incorrect, return an empty transition matrix object
if ( rate.size() != nSt*(nSt-1) )
return;
// initialize
numStates = nSt;
// Allocate space for the stationary frequencies
pi = std::vector<double>(numStates, 0.0);
// Allocate space for variables that hold Q
Q = MbMatrix<double>(numStates, numStates, 0.0);
oldQ = MbMatrix<double>(numStates, numStates, 0.0);
// Allocate space for eigensystem calculations
if (useEigens) {
allocateEigens();
}
setPadeTolerance(1E-6);
// Update the Q matrix and the precalculated values
// Do it twice to make sure restore always works
updateQ(rate);
updateQ(rate);
}
/*!
* This constructor builds a time-reversible transition matrix to describe
* evolutionary substitutions along a phylogenetic tree for discrete characters.
* Such a model is often used to describe substitutions for DNA or amino acid data
* but it works equally well for other characters with discrete states.
*
* \brief Creates time-reversible transition matrix
* \param rate Vector of rates
* \param pi Vector of stationary state frequencies
* \param useEigen Use eigensystem (true) or Pade approximation (false)
*
*/
MbTransitionMatrix::MbTransitionMatrix(const std::vector<double> &rate, const std::vector<double> &pi, bool useEigen)
: c_ijk(0), cc_ijk(0), ceigenvalue(0), ceigValExp(0), eigens(0), eigenvalue(0), eigValExp(0),
isComplex(false), isOldComplex(false), isRev(true), numStates(0), oldC_ijk(0), oldEigenvalue(0),
oldCC_ijk(0), oldCEigenvalue(0), useEigens(useEigen), hasUniformizedMatrix(false) {
// Check for consistency
int nSt = pi.size();
if ( 2*rate.size() != nSt*(nSt-1) )
return; // return empty transition matrix
// Initialize
numStates = nSt;
// Store the stationary frequencies in case someone asks for them
this->pi = pi;
// Allocate space for variables that hold Q
Q = MbMatrix<double>(numStates, numStates, 0.0);
oldQ = MbMatrix<double>(numStates, numStates, 0.0);
// Allocate space for eigensystem calculations
if (useEigens)
allocateEigens();
setPadeTolerance(1E-6);
// Update the Q matrix and the precalculated values
// Do it twice to ensure that restore always works
updateQ(rate, pi);
updateQ(rate, pi);
}
wYangModel::wYangModel(const MDOUBLE inW, const MDOUBLE inK, const Vdouble& freq,bool globalW, codon * coAlph):
_w(inW),_k(inK),_globalW(globalW),_freq(freq),_coAlpha(NULL){
_coAlpha = (codon*)(coAlph->clone());
_Q.resize(alphabetSize());
codonUtility::initSubMatrices(*_coAlpha);
for (int z=0; z < _Q.size();++z) _Q[z].resize(alphabetSize(),0.0);
updateQ();
}
StereoCameraModel::StereoCameraModel(const StereoCameraModel& other)
: left_(other.left_), right_(other.right_),
Q_(0.0)
{
Q_(0,0) = Q_(1,1) = 1.0;
if (other.initialized())
updateQ();
}
void Spline::updateM(int lvl)
{
if (m_p.size() < 4)
{
m_q.clear();
return;
}
// Every segment is subdivided lvl times
//
int f = pow(2.0,lvl)+1;
VectorXd ranges(f);
// Ranges from 0 to 1
//
for (int i=0; i<f; ++i)
ranges(i) = (double)i/(f-1.0);
// Compute the subdivision matrix for a single segment
//
MatrixXd sM = MatrixXd::Zero(f,4);
MatrixXd A(4,4);
A << -1, 3, -3, 1,
3, -6, 3, 0,
-3, 0, 3, 0,
1, 4, 1, 0;
for (int i=0; i<f; ++i)
{
double t = ranges(i);
MatrixXd tm(4,1); tm << t*t*t, t*t, t, 1;
MatrixXd temp = tm.transpose()*(1.0/6.0)*A;
for(int j=0;j<4;++j)
sM(i,j) = temp(j);
}
int segmentCount = (int)m_p.size()-3;
int finalPointCount = (segmentCount * f) - (segmentCount-1);
M = MatrixXd::Zero(finalPointCount, m_p.size());
// Produce the set of final points and the global subdivision matrix
//
for (size_t m=1; m<m_p.size()-2; ++m)
{
int blockN = m-1;
int i = blockN*(f-1);
int j = blockN;
M.block(i, j, sM.rows(), sM.cols()) = sM;
}
m_q.resize(finalPointCount);
updateQ();
}
gtrModel::gtrModel(const Vdouble& freq,
const MDOUBLE a2c,
const MDOUBLE a2g,
const MDOUBLE a2t,
const MDOUBLE c2g,
const MDOUBLE c2t,
const MDOUBLE g2t)
:_a2c(a2c),_a2g(a2g),_a2t(a2t),_c2g(c2g),_c2t(c2t),_g2t(g2t),_freq(freq)
{
_Q.resize(alphabetSize());
for (int z=0; z < _Q.size();++z) _Q[z].resize(alphabetSize(),0.0);
updateQ(a2c,a2g,a2t,c2g,c2t,g2t);
}
/*!
* This constructor builds a time-reversible transition matrix to describe
* evolutionary substitutions along a phylogenetic tree for discrete characters.
* Such a model is often used to describe substitutions for DNA or amino acid data
* but it works equally well for other characters with discrete states.
*
* \brief Creates time-reversible transition matrix
* \param rate Vector of rates
* \param pi Vector of stationary state frequencies
* \param useEigen Use eigensystem (true) or Pade approximation (false)
*
*/
MbTransitionMatrix::MbTransitionMatrix(const MbMatrix<double> &qMat, bool useEigen)
: c_ijk(0), cc_ijk(0), ceigenvalue(0), ceigValExp(0), eigens(0), eigenvalue(0), eigValExp(0),
isComplex(false), isOldComplex(false), isRev(true), numStates(0), oldC_ijk(0), oldEigenvalue(0),
oldCC_ijk(0), oldCEigenvalue(0), useEigens(useEigen), hasUniformizedMatrix(false) {
// Initialize
numStates = qMat.dim1();
// Allocate space for variables that hold Q
Q = MbMatrix<double>(numStates, numStates, 0.0);
oldQ = MbMatrix<double>(numStates, numStates, 0.0);
// Allocate space for eigensystem calculations
if (useEigens)
allocateEigens();
setPadeTolerance(1E-6);
// Update the Q matrix and the precalculated values
// Do it twice to ensure that restore always works
updateQ(qMat);
updateQ(qMat);
}
bool StereoCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& left,
const sensor_msgs::CameraInfo& right)
{
bool changed_left = left_.fromCameraInfo(left);
bool changed_right = right_.fromCameraInfo(right);
bool changed = changed_left || changed_right;
// Note: don't require identical time stamps to allow imperfectly synced stereo.
assert( left_.tfFrame() == right_.tfFrame() );
assert( left_.fx() == right_.fx() );
assert( left_.fy() == right_.fy() );
assert( left_.cy() == right_.cy() );
// cx may differ for verged cameras
if (changed)
updateQ();
return changed;
}
MainWindow::MainWindow(QWidget *parent)
: QMainWindow(parent), ui(new Ui::MainWindow)
{
ui->setupUi(this);
env = new Environment();
agent = new Agent(env, 0.5, 0.5, 0.1); // double alpha, double gamma, double eps
ui->alphaCombo->setCurrentIndex(5);
ui->gammaCombo->setCurrentIndex(5);
ui->epsilonCombo->setCurrentIndex(4);
connect(env, SIGNAL(statusChanged(Status)),
this, SLOT(recvStatusChanged(Status)));
connect(ui->alphaCombo, SIGNAL(currentIndexChanged(int)),
agent, SLOT(changeAlpha(int)));
connect(ui->gammaCombo, SIGNAL(currentIndexChanged(int)),
agent, SLOT(changeGamma(int)));
connect(ui->epsilonCombo, SIGNAL(currentIndexChanged(int)),
agent, SLOT(changeEps(int)));
ui->sView->setQ(&(agent->m_Q));
connect(agent, SIGNAL(updateQ()),
ui->sView, SLOT(update()));
}
int main()
{
//learning factor
const double alpha = 0.5;
//discount factor
const double gamma = 0.5;
const double deltatime = 0.05;
const double mass = 0.5;
const double length = 0.08;
const int angle_bins = 100;
const int velocity_bins = 50;
const int torque_bins = 9;
const double max_angle = M_PI;
const double max_velocity = 10;
const double max_torque = 4;
Environment* env = new Environment(0, 0, 0, max_torque, 0, deltatime, mass, length, gamma);
//seed rng
std::srand(static_cast<unsigned int>(std::time(NULL)));
//create the possible actions as well as the chosen action
float chosen_action = 1; // rip f
float actions[torque_bins];
for (int i = 0; i < (max_torque*2)+1 ; ++i)
{
actions[i] = static_cast<float>(-max_torque+i);
}
//create a priority queue to copy to all the state space priority queues
PriorityQueue<float, double> initiator_queue(MAX);
for (int i = 0; i < torque_bins; ++i)
{
initiator_queue.enqueueWithPriority(actions[i], 0);
}
//create the state space
StateSpace space(initiator_queue, angle_bins, velocity_bins, torque_bins, max_angle, max_velocity, max_torque);
//state objects
State current_state(0, 0, 0);
State old_state(0, 0, 0);
// std::ofstream file("output.txt");
// file.precision(16);
// file << "Trialno" << " " << "Time" << " " << "Theta" << " " << "Thetadot" << " " << "Torque" << std::endl;
double trialno = 1;
unsigned long i=0;
while (true)
{
current_state.theta = env->getTheta();
current_state.theta_dot = env->getThetadot();
current_state.torque = env->getTorque();
std::cout << "State Read" << std::endl;
updateQ(space, chosen_action, old_state, current_state, alpha, gamma);
if (std::abs(current_state.theta_dot) > 5)
{
env->resetPendulum();
std::cout<<"->unsuccessful trial\n";
trialno++;
i = 0;
continue;
}
old_state = current_state;
chosen_action = selectAction(space[current_state],i);
std::cout << "Action Selected" << std::endl;
env->setTorque(chosen_action);
env->propagate();
std::cout << "Environment Propagated\n" << std::endl;
// file << trialno << " " << env->getTime() << " " << current_state.theta << " " << current_state.theta_dot << " " << current_state.torque << std::endl;
++i;
}
// file.close();
delete env;
return 1;
}
void gtrModel::set_a2g(const MDOUBLE a2g)
{
_a2g = a2g;
updateQ(_a2c,_a2g,_a2t,_c2g,_c2t,_g2t);
}
void gtrModel::set_a2c(const MDOUBLE a2c)
{
_a2c = a2c;
updateQ(_a2c,_a2g,_a2t,_c2g,_c2t,_g2t);
}
void gtrModel::set_g2t(const MDOUBLE g2t)
{
_g2t = g2t;
updateQ(_a2c,_a2g,_a2t,_c2g,_c2t,_g2t);
}
