void AgentCore::control() {
Eigen::VectorXd stats_error = statsMsgToVector(target_statistics_) - statsMsgToVector(estimated_statistics_);
// update non constant values of the jacobian of phi(p) = [px, py, pxx, pxy, pyy]
jacob_phi_(2,0) = 2*pose_virtual_.position.x;
jacob_phi_(3,0) = pose_virtual_.position.y;
jacob_phi_(3,1) = pose_virtual_.position.x;
jacob_phi_(4,1) = 2*pose_virtual_.position.y;
// twist_virtual = inv(B + Jphi'*lambda*Jphi) * Jphi' * gamma * stats_error
Eigen::VectorXd control_law = (b_ + jacob_phi_.transpose()*lambda_*jacob_phi_).inverse()
*jacob_phi_.transpose()*gamma_*stats_error;
// control command saturation
double current_velocity_virtual = std::sqrt(std::pow(control_law(0),2) + std::pow(control_law(1),2));
if (current_velocity_virtual > velocity_virtual_threshold_) {
control_law *= velocity_virtual_threshold_ / current_velocity_virtual;
}
geometry_msgs::Pose pose_old = pose_virtual_;
pose_virtual_.position.x = integrator(pose_virtual_.position.x, twist_virtual_.linear.x, control_law(0), 1);
pose_virtual_.position.y = integrator(pose_virtual_.position.y, twist_virtual_.linear.y, control_law(1), 1);
setTheta(pose_virtual_.orientation, std::atan2(control_law(1), control_law(0)));
twist_virtual_.linear.x = control_law(0);
twist_virtual_.linear.y = control_law(1);
std::stringstream s;
s << "Statistics error (" << (Eigen::RowVectorXd)stats_error << ").";
console(__func__, s, DEBUG_V);
s << "Control commands (" << (Eigen::RowVectorXd)control_law << ").";
console(__func__, s, DEBUG_VV);
s << "Virtual pose (" << pose_virtual_.position.x << ", " << pose_virtual_.position.y << ").";
console(__func__, s, DEBUG_VVV);
s << "Virtual twist (" << twist_virtual_.linear.x << ", " << twist_virtual_.linear.y << ").";
console(__func__, s, DEBUG_VVV);
broadcastPose(pose_virtual_, agent_virtual_frame_);
broadcastPath(pose_virtual_, pose_old, agent_virtual_frame_);
}
int testBelugaBoundaryControlLaw()
{
const char* bad_file_x = "bad_file.txt";
const char* bad_file_y = "bad_file.txt";
const char* good_file_x = "../src/Boundary_ForcesX.txt";
const char* good_file_y = "../src/Boundary_ForcesY.txt";
BelugaBoundaryControlLaw control_law(good_file_x, good_file_y);
/* should fail to load bad files */
DO_TEST("Checking for failure on loading bad files (x)",
control_law.loadForceFiles(bad_file_x, good_file_y),
"Didn't return false for " << bad_file_x);
DO_TEST("Checking for failure on loading bad files (y)",
control_law.loadForceFiles(good_file_x, bad_file_y),
"Didn't return false for " << bad_file_y);
DO_TEST("Checking for success on loading good files",
!control_law.loadForceFiles(good_file_x, good_file_y),
"Unable to load files " << good_file_x << " and " << good_file_y);
std::string err_msg;
mt_dVector_t u_in, u_out, state;
u_in.resize(BELUGA_CONTROL_SIZE, 0.0);
state.resize(BELUGA_NUM_STATES, 0.0);
/* the output should have BELUGA_CONTROL_SIZE elements */
START_TEST("Checking output vector size");
u_out = control_law.doControl(state, u_in);
if (u_out.size() != BELUGA_CONTROL_SIZE)
RETURN_ERROR_ELSE_OK("Incorrect size " << u_out.size());
/* robot speed should not change at origin */
START_TEST("Checking origin");
state[BELUGA_STATE_X] = 0.0;
state[BELUGA_STATE_Y] = 0.0;
double u_speed = u_in[BELUGA_CONTROL_FWD_SPEED];
double u_vert = u_in[BELUGA_CONTROL_VERT_SPEED];
double u_turn = u_in[BELUGA_CONTROL_STEERING];
u_out = control_law.doControl(state, u_in);
double d_speed = fabs(u_speed - u_out[BELUGA_CONTROL_FWD_SPEED]);
double d_vert = fabs(u_vert - u_out[BELUGA_CONTROL_VERT_SPEED]);
double d_turn = fabs(u_turn - u_out[BELUGA_CONTROL_STEERING]);
if (!eq_wf(d_speed, 0.0))
RETURN_ERROR_ELSE_OK("Forward speed not zero, was " << u_out[BELUGA_CONTROL_FWD_SPEED]);
if (!eq_wf(d_vert, 0.0))
RETURN_ERROR_ELSE_OK("Vertical speed not zero, was " << u_out[BELUGA_CONTROL_VERT_SPEED]);
if (!eq_wf(d_turn, 0.0))
RETURN_ERROR_ELSE_OK("Steering not zero, was " << u_out[BELUGA_CONTROL_STEERING]);
/* check robot's response to boundaries in x and y (up to buffer zone), along the respective axes */
double minDist = 0.5;
double step = 0.1;
double boundary = DEFAULT_TANK_RADIUS/sqrt((double) 2.0);
double buffer = 0.25;
/* robot speed should decrease in x and not change in y the closer it gets to the +x boundary */
START_TEST("Checking +x boundary");
state[BELUGA_STATE_Y] = 0.0;
state[BELUGA_STATE_THETA] = 0.0;
double th = state[BELUGA_STATE_THETA];
u_speed = u_in[BELUGA_CONTROL_FWD_SPEED];
u_turn = u_in[BELUGA_CONTROL_STEERING];
double prev_vx = u_speed*cos(th) - u_turn*sin(th);
double prev_vy = u_speed*sin(th) + u_turn*cos(th);
double dist = minDist;
while (dist < boundary - buffer)
{
state[BELUGA_STATE_X] = dist;
u_out = control_law.doControl(state, u_in);
double vr = u_out[BELUGA_CONTROL_FWD_SPEED];
double vth = u_out[BELUGA_CONTROL_STEERING];
double vx = vr*cos(th) - vth*sin(th);
double vy = vr*sin(th) + vth*cos(th);
if (vx > prev_vx)
RETURN_ERROR_ELSE_OK("Speed did not decrease in x, was " << vx);
double dvy = fabs(vy - prev_vy);
if (!eq_wf(dvy, 0.0))
RETURN_ERROR_ELSE_OK("Y-speed changed, d_vy was " << dvy);
prev_vx = vx;
prev_vy = vy;
dist += step;
}
/* robot speed should increase in x and not change in y the closer it gets to the -x boundary */
START_TEST("Checking -x boundary");
state[BELUGA_STATE_Y] = 0.0;
state[BELUGA_STATE_THETA] = PI;
th = state[BELUGA_STATE_THETA];
u_speed = u_in[BELUGA_CONTROL_FWD_SPEED];
u_turn = u_in[BELUGA_CONTROL_STEERING];
prev_vx = u_speed*cos(th) - u_turn*sin(th);
prev_vy = u_speed*sin(th) + u_turn*cos(th);
dist = -minDist;
while (dist > -(boundary - buffer))
{
state[BELUGA_STATE_X] = dist;
u_out = control_law.doControl(state, u_in);
double vr = u_out[BELUGA_CONTROL_FWD_SPEED];
double vth = u_out[BELUGA_CONTROL_STEERING];
double vx = vr*cos(th) - vth*sin(th);
double vy = vr*sin(th) + vth*cos(th);
if (vx < prev_vx)
RETURN_ERROR_ELSE_OK("Speed did not increase in x, was " << vx);
double dvy = fabs(vy - prev_vy);
if (!eq_wf(dvy, 0.0))
RETURN_ERROR_ELSE_OK("Y-speed changed, d_vy was " << dvy);
prev_vx = vx;
prev_vy = vy;
dist -= step;
}
/* robot speed should decrease in y and not change in x the closer it gets to the +y boundary */
START_TEST("Checking +y boundary");
state[BELUGA_STATE_X] = 0.0;
state[BELUGA_STATE_THETA] = PI/2;
th = state[BELUGA_STATE_THETA];
u_speed = u_in[BELUGA_CONTROL_FWD_SPEED];
u_turn = u_in[BELUGA_CONTROL_STEERING];
prev_vx = u_speed*cos(th) - u_turn*sin(th);
prev_vy = u_speed*sin(th) + u_turn*cos(th);
dist = minDist;
while (dist < boundary - buffer)
{
state[BELUGA_STATE_Y] = dist;
u_out = control_law.doControl(state, u_in);
double vr = u_out[BELUGA_CONTROL_FWD_SPEED];
double vth = u_out[BELUGA_CONTROL_STEERING];
double vx = vr*cos(th) - vth*sin(th);
double vy = vr*sin(th) + vth*cos(th);
if (vy > prev_vy)
RETURN_ERROR_ELSE_OK("Speed did not decrease in y, was " << vy);
double dvx = fabs(vx - prev_vx);
if (!eq_wf(dvx, 0.0))
RETURN_ERROR_ELSE_OK("X-speed changed, d_vy was " << dvx);
prev_vx = vx;
prev_vy = vy;
dist += step;
}
/* robot speed should increase in y and not change in x the closer it gets to the -y boundary */
START_TEST("Checking -y boundary");
state[BELUGA_STATE_X] = 0.0;
state[BELUGA_STATE_THETA] = (3/2)*PI;
th = state[BELUGA_STATE_THETA];
u_speed = u_in[BELUGA_CONTROL_FWD_SPEED];
u_turn = u_in[BELUGA_CONTROL_STEERING];
prev_vx = u_speed*cos(th) - u_turn*sin(th);
prev_vy = u_speed*sin(th) + u_turn*cos(th);
dist = -minDist;
while (dist > -(boundary - buffer))
{
state[BELUGA_STATE_Y] = dist;
u_out = control_law.doControl(state, u_in);
double vr = u_out[BELUGA_CONTROL_FWD_SPEED];
double vth = u_out[BELUGA_CONTROL_STEERING];
double vx = vr*cos(th) - vth*sin(th);
double vy = vr*sin(th) + vth*cos(th);
if (vy < prev_vy)
RETURN_ERROR_ELSE_OK("Speed did not increase in y, was " << vy);
double dvx = fabs(vx - prev_vx);
if (!eq_wf(dvx, 0.0))
RETURN_ERROR_ELSE_OK("X-speed changed, d_vy was " << dvx);
prev_vx = vx;
prev_vy = vy;
dist -= step;
}
return OK;
}
