void playerScore(const StateMatrix& state,
const ControlData& control,
const Map& map,
const SimulationParameters& params,
GameStats& gameStats)
{
std::vector<float> distance(state.rows());
for (uint i=0; i<state.rows();i++)
{
std::pair<uint,uint> coords = indices(state(i,0), state(i,1), map, params);
distance[i] = map.endDistance(coords.first, coords.second);
gameStats.playerDists[control.ids.at(i)] = distance[i];
}
std::vector<int> indices = argsort(distance);
std::vector<int> ranks(indices.size());
for (int i=0;i<indices.size();i++)
ranks[indices[i]] = i;
for (uint i=0; i<state.rows();i++)
{
gameStats.playerRanks[control.ids.at(i)] = ranks[i];
}
}
LqController() : alpha(1.0) {
A.setZero();
B.setZero();
u.setZero();
L.setZero();
Q.setIdentity();
R.setIdentity();
resetRiccati();
}
LQRController::UpdateGainsJob::UpdateGainsJob(LQRController * c, const StateMatrix & weights, const InputMatrix &input_weights, const StateVector & target)
: c_(c), target_(target)
{
if(!c)
return;
ROS_DEBUG_STREAM("target is "<<target.transpose());
const int n=weights.rows();
const int m=input_weights.rows();
input_offset_.resize(m,1);
gains_.resize(n,n);
//Computes a linearization around the target point
StateMatrix A(n,n);
InputMatrix B(n,m);
if(!c_->model_->getLinearization(target, A, B, input_offset_))
{
ROS_ERROR("Failed to get linearization");
c_=NULL;
return;
}
ROS_DEBUG_STREAM("************ Obtained linearization **********\n[A]\n"<<A<<std::endl<<"[B]"<<std::endl<<B<<std::endl<<"[input offset]"<<std::endl<<input_offset_<<"\n*************");
ROS_DEBUG("Sanity checks");
ROS_ASSERT(LQR::isCommandable(A,B));
// Compute new gains matrix
ROS_DEBUG("Computing LQR gains matrix. Hold on to your seatbelts...");
LQR::LQRDP<StateMatrix,InputMatrix,StateMatrix,InputMatrix>::runContinuous(A, B, weights, weights, input_weights, 0.01, 0.1, gains_);
ROS_DEBUG_STREAM("Done. Computed gains:\n***********\n"<<gains_<<"\n**********\n");
// Check validity
ROS_DEBUG("Checking gains matrix...");
bool test=LQR::isConverging(A,B,gains_);
ROS_ASSERT(test);
ROS_DEBUG("Valid gains matrix");
// assert(test);
}
void updateControlMatrices() {
Scalar measure = 1000;
Scalar newMeasure;
if(isDiscrete) {
auto Rinv = R.householderQr();
auto BRB = B*Rinv.solve(B.transpose());
StateMatrix deltaP;
do {
deltaP = alpha*(A.transpose()*P + P*A - P*BRB*P + Q).householderQr().solve(P);
P += deltaP;
newMeasure = deltaP.cwiseAbs2().maxCoeff();
//~ std::cout << "(c) Iteration! (" << newMeasure << ")" << std::endl;
if(newMeasure > measure) {
//~ std::cout << "(c) Wrong way!" << std::endl;
break;
}
measure = newMeasure;
} while(measure > P_DELTA_EPSILON);
L = Rinv.solve(B.transpose()*P);
} else {
StateMatrix Pprev;
//~ std::cout << "Eig(A): :::::::::::::::::::::::::::" << std::endl << A.eigenvalues().transpose() << std::endl;
do {
Pprev = P;
P = Q + A.transpose()*(P - P*B*(R + B.transpose()*P*B).ldlt().solve(B.transpose()*P))*A;
newMeasure = (P-Pprev).cwiseAbs2().maxCoeff();
if(newMeasure > measure) {
//~ std::cout << "(d) Wrong way!" << std::endl;
break;
}
measure = newMeasure;
} while(measure > P_DELTA_EPSILON);
L = (R + B.transpose()*P*B).ldlt().solve(B.transpose()*P*A);
}
LOG_EVENT(typeid(Self).name(), 50, "L::::::::::::::\n" << L << "\n:::::::::::::::::::::::::::::::::::::" << std::endl);
}
void rk4TimeStep(StateMatrix& s, const ControlMatrix& c, const
SimulationParameters& params, const Map & m)
{
// CHANGED(AL): Now implementing RK4
StateMatrix k1, k2, k3, k4;
uint n_states = s.cols();
uint n = s.rows();
k1 = StateMatrix(n, n_states);
k2 = StateMatrix(n, n_states);
k3 = StateMatrix(n, n_states);
k4 = StateMatrix(n, n_states);
float dt = params.timeStep;
derivatives(s, k1, c, params, m);
derivatives(s + 0.5*k1*dt, k2, c, params, m);
derivatives(s + 0.5*k2*dt, k3, c, params, m);
derivatives(s + k3*dt, k4, c, params, m);
s += (k1 + 2.*k2 + 2.*k3 + k4) * dt/6.;
}
std::string winner(const PlayerSet& players,
const StateMatrix& state,
const ControlData& control,
const Map& map,
const SimulationParameters& params)
{
std::string winnerName = "";
for (uint i=0; i<state.rows();i++)
{
std::pair<uint,uint> coords = indices(state(i,0), state(i,1), map, params);
bool winner = map.finish.count(coords);
if (winner)
{
winnerName = control.ids.at(i);
break;
}
}
return winnerName;
}
void initialiseState(StateMatrix& state, const Map& map,
const SimulationParameters& params)
{
static std::default_random_engine gen;
static std::uniform_real_distribution<float> dist(0.0, 1.0);
std::vector<std::pair<uint,uint>> startPixels;
std::copy(map.start.begin(), map.start.end(), std::back_inserter(startPixels));
std::random_shuffle(startPixels.begin(), startPixels.end());
uint pixelIdx = 0;
for (uint i=0; i<state.rows();i++)
{
auto coords = startPixels[pixelIdx];
state(i, 0) = (float(coords.second) + dist(gen)) / params.mapScale;
state(i, 1) = (float(coords.first) + dist(gen)) / params.mapScale;
state(i, 2) = 0.0;
state(i, 3) = 0.0;
state(i, 4) = dist(gen) * 2.0 * M_PI;
state(i, 5) = 0.0;
pixelIdx = (pixelIdx + 1) % startPixels.size();
}
}
void derivatives(const StateMatrix& states, StateMatrix& derivs,
const ControlMatrix& ctrls, const SimulationParameters& params,
const Map & map)
{
float cd_a_rho = params.linearDrag; // 0.1 coeff of drag * area * density of fluid
float k_elastic = params.elasticity; // 4000. // spring constant of ships
float rad = 1.; // leave radius 1 - we decided to change map scale instead
const Eigen::VectorXf &masses = params.shipDensities; // order of 1.0 Mass of ships
float spin_drag_ratio = params.rotationalDrag; // 1.8; // spin friction to translation friction
float eps = 1e-5; // Avoid divide by zero special cases
float mu = params.shipFriction; // 0.05; // friction coefficient between ships
float mu_wall = params.wallFriction; //0.25*?wallFriction; // 0.01; // wall friction parameter
float wall_restitution = params.wallRestitution; // 0.5
float ship_restitution = params.shipRestitution; // circa 0.5
float diameter = 2.*rad; // rad(i) + rad(j) for any i, j
float inertia_mass_ratio = 0.25;
float map_grid = rad * 2. + eps; // must be 2*radius + eps
std::unordered_map<std::pair<int, int>, std::vector<uint>,
boost::hash<std::pair<int, int>>> bins;
uint n = states.rows();
Eigen::MatrixXd f = Eigen::MatrixXd::Zero(n, 2);
Eigen::VectorXd trq = Eigen::VectorXd::Zero(n);
// rotationalThrust Order +- 10
// linearThrust Order +100
// mapscale order 10 - thats params.pixelsize
// Accumulate forces and torques into these:
uint collide_checks = 0; // debug count...
for (uint i=0; i<n; i++) {
Eigen::Vector2f pos_i;
pos_i(0) = states(i,0);
pos_i(1) = states(i,1);
Eigen::Vector2f vel_i;
vel_i(0) = states(i,2);
vel_i(1) = states(i,3);
float theta_i = states(i,4);
float w_i = states(i,5);
// 1. Control
float thrusting = ctrls(i, 0);
float turning = ctrls(i, 1);
f(i, 0) = thrusting * params.linearThrust * cos(theta_i);
f(i, 1) = thrusting * params.linearThrust * sin(theta_i);
trq(i) = turning * params.rotationalThrust;
// 2. Drag
f(i, 0) -= cd_a_rho * vel_i(0);
f(i, 1) -= cd_a_rho * vel_i(1);
trq(i) -= spin_drag_ratio*cd_a_rho*w_i*rad*rad; // * abs(w_i)
// 3. Inter-ship collisions against ships of lower index...
// Figure out this ship's hashes: It has 4 in 2 dimensions
std::unordered_set<uint> collision_shortlist;
std::pair<int, int> my_hash;
for (int dx=-1; dx < 2; dx+=2)
for (int dy=-1; dy < 2; dy+=2)
{
float x_mod = pos_i(0) + float(dx)*rad;
float y_mod = pos_i(1) + float(dy)*rad;
my_hash = std::make_pair(int(x_mod / map_grid),
int(y_mod / map_grid));
if (bins.count(my_hash) > 0)
{
// Already exists - shortlist others and add self
std::vector<uint> current_bin = bins.find(my_hash)->second;
// -->first is the key as it returns a key/value pair
for (uint bin_idx: current_bin)
if (bin_idx != i)
collision_shortlist.insert(bin_idx);
current_bin.push_back(i);
}
else
{
// didnt exist - add self, and push into map
std::vector<uint> current_bin;
current_bin.push_back(i);
bins.insert(std::make_pair(my_hash, current_bin));
}
}
for (uint j: collision_shortlist) { // =i+1; j<n; j++) {
collide_checks ++;
// std::cout << "Checking " << i << ", " << j << "\n";
Eigen::Vector2f pos_j;
pos_j(0) = states(j,0);
pos_j(1) = states(j,1);
Eigen::Vector2f vel_j;
vel_j(0) = states(j,2);
vel_j(1) = states(j,3);
float theta_j = states(j,4);
float w_j = states(j,5);
Eigen::Vector2f dP = pos_j - pos_i;
float dist = dP.norm() + eps - diameter;
Eigen::Vector2f dPhat = dP / (dP.norm() + eps);
if (dist < 0) {
// we have a collision interaction
// A. Direct collision: apply linear spring normal force
float f_magnitude = - dist * k_elastic; // dist < =
if ((vel_j - vel_i).dot(pos_j - pos_i) > 0)
f_magnitude *= ship_restitution;
Eigen::Vector2f f_norm = f_magnitude * dPhat;
f(i, 0) -= f_norm(0);
f(i, 1) -= f_norm(1);
f(j, 0) += f_norm(0);
f(j, 1) += f_norm(1);
// B. Surface frictions: approximate spin effects
Eigen::Vector2f perp; // surface tangent pointing +theta direction
perp(0) = -dPhat(1);
perp(1) = dPhat(0);
// relative velocities of surfaces
float v_rel = rad*w_i + rad*w_j + perp.dot(vel_i - vel_j);
float fric = f_magnitude * mu * sigmoid(v_rel);
Eigen::Vector2f f_fric = fric * perp;
f(i, 0) += f_fric(0);
f(i, 1) += f_fric(1);
f(j, 0) -= f_fric(0);
f(j, 1) -= f_fric(1);
trq(i) -= fric * rad;
trq(j) -= fric * rad;
} // end collision
} // end loop 3. opposing ship
// 4. Wall single body collisions
// compute distance to wall and local normals
float wall_dist, norm_x, norm_y;
interpolate_map(pos_i(0), pos_i(1), wall_dist, norm_x, norm_y, map, params);
float dist = wall_dist - rad;
if (dist < 0)
{
/* if (dist < -1.) */
/* assert(false); */
// Spring force
float f_norm_mag = -dist*k_elastic; // dist is negative, f_norm is +ve
if (norm_x*vel_i(0) + norm_y*vel_i(1) > 0)
f_norm_mag *= wall_restitution;
if (dist > -rad*0.25)
{
// not significantly through wall yet
f(i, 0) += f_norm_mag * norm_x;
f(i, 1) += f_norm_mag * norm_y;
}
else
{
// uh-oh - lets just SET normal forces and seriously damp vel
f(i, 0) = f_norm_mag * norm_x;
f(i, 1) = f_norm_mag * norm_y;
f(i, 0) -= 100. * vel_i(0);
f(i, 1) -= 100. * vel_i(1);
}
// Surface friction
Eigen::Vector2f perp; // surface tangent pointing +theta direction
perp(0) = -norm_y;
perp(1) = norm_x;
float v_rel = w_i * rad + vel_i(0)*norm_y - vel_i(1)*norm_x;
float fric = f_norm_mag * mu_wall * sigmoid(v_rel);
f(i, 0) -= fric*norm_y;
f(i, 1) += fric*norm_x;
trq(i) -= fric * rad;
}
} // end loop current ship
// std::cout << "Collision checks:" << collide_checks << "\n";
// Compose the vector of derivatives:
float vmax = 40.0;
for (int i=0; i<n; i++)
{
float vx = states(i,2);
float vy = states(i,3);
float speed = std::sqrt(vx*vx + vy*vy);
if (speed > vmax)
{
vx *= vmax/speed;
vy *= vmax/speed;
}
// x_dot = vx
derivs(i, 0) = vx;
// y_dot = vy
derivs(i, 1) = vy;
// vx_dot = fx / m
float ax = f(i,0)/masses(i);
float ay = f(i,1)/masses(i);
derivs(i, 2) = ax;
// vy_dot = fy / m
derivs(i, 3) = ay;
// theta_dot = omega
derivs(i, 4) = states(i, 5);
// omega_dot = T_r / (inertia_mass_ratio*m)
derivs(i,5) = trq(i) / (inertia_mass_ratio * masses(i));
}
// ux uy vx vy theta omega
// 0 1 2 3 4 5
}
// General outline: the model can be written:
// M(o)o_dotdot + Q(o,o_dot)o_dot + G(o) = tau
// When we linearize to the first order around position O, we get the approximation: o = O + e and
// M(O)e_dotdot + Q(O,O_dot)e_dot + G(O) + grad(G)(O)e = tau
// Or:
// (e)_dot = I e_dot + 0
// (e_dot)_dot = -M^-1 Q e_dot -M^-1 G e + M^-1 tau - G(O)
// We first compute the matrices M,Q,G and then linearize the gravity term.
bool SerialChainModel::getLinearization(const StateVector & x, StateMatrix & A, InputMatrix & B, InputVector & c)
{
// ROS_DEBUG("1");
assert(inputs_>0);
const int n=inputs_;
KDL::JntArray kdl_q(n);
KDL::JntArray kdl_q_dot(n);
// ROS_DEBUG("2");
//Fills the data
for(int i=0; i<n; ++i)
{
kdl_q(i) = x(i);
kdl_q_dot(i) = x(i+n);
}
// ROS_DEBUG("3");
//Compute M
Eigen::MatrixXd M(n,n);
NEWMAT::Matrix mass(n,n);
chain_->computeMassMatrix(kdl_q,kdl_torque_,mass);
for(int i=0;i<n;i++)
for(int j=0;j<n;j++)
M(i,j)=mass(i+1,j+1);
// ROS_DEBUG("4");
//Compute Q
Eigen::MatrixXd Q(n,n);
Q.setZero();
NEWMAT::Matrix christoffel(n*n,n);
chain_->computeChristoffelSymbols(kdl_q,kdl_torque_,christoffel);
// ROS_DEBUG("5-");
for(int i=0;i<n;i++)
for(int j=0;j<n;j++)
for(int k=0;k<n;k++)
Q(i,j)+=christoffel(i*n+j+1,k+1)*kdl_q_dot(j);
// ROS_DEBUG("5");
//Compute G
Eigen::VectorXd G(n,1);
chain_->computeGravityTerms(kdl_q,kdl_torque_);
for(int i=0;i<n;i++)
G(i)=kdl_torque_[i][2];
// ROS_DEBUG("6");
//Computing the gradient of G
Eigen::MatrixXd gG(n,n);
const double epsi=0.01;
for(int i=0;i<n;i++)
{
KDL::JntArray ja = kdl_q;
ja(i)=ja(i)+epsi;
chain_->computeGravityTerms(ja,kdl_torque_);
for(int j=0;j<n;j++)
gG(i,j)=(kdl_torque_[j][2]-G(j))/epsi;
}
// ROS_DEBUG("7");
// Final assembling
Eigen::MatrixXd Minvminus=-M.inverse(); //Computing M's inverse once
ROS_DEBUG_STREAM(A.rows());
assert(A.rows()==n*2);
assert(A.cols()==n*2);
A.block(0,0,n,n).setZero();
A.block(0,n,n,n)=Eigen::MatrixXd::Identity(n,n);
A.block(n,0,n,n)=Minvminus*gG;
A.block(n,n,n,n)=Minvminus*Q;
assert(B.rows()==n*2);
assert(B.cols()==n);
B.block(0,0,n,n).setZero();
B.block(n,0,n,n)=Minvminus;
// ROS_DEBUG("8");
c=-G;
return true;
}
