void Infinite::TA_calculateE0(int nA, double (Infinite::*func)(), double (Infinite::*ref)())
{
double refValue = (this->*ref)();
double SP_diff = abs(refValue);
SP_deltaE = 0.0;
TA_deltaE = 0.0;
twisted_x.resize(nA);
twisted_w.resize(nA);
gauleg(0,M_PI,twisted_x,twisted_w);
for (int x = 0; x < twisted_x.size(); x++)
for (int y = 0; y < twisted_x.size(); y++)
for (int z = 0; z < twisted_x.size(); z++)
{
setTheta(twisted_x[x],twisted_x[y],twisted_x[z]);
double value = (this->*func)();
TA_deltaE += twisted_w[x] * twisted_w[y] * twisted_w[z] * value;
if (abs(value/A - refValue) < SP_diff)
{
SP_diff = abs(value/A - refValue);
SP_x = x;
SP_y = y;
SP_z = z;
SP_deltaE = value;
}
}
TA_deltaE /= pow(M_PI,3);
setTheta(0.0,0.0,0.0);
}
EulerAngles::EulerAngles(float phi, float theta, float psi) :
Vector(3)
{
setPhi(phi);
setTheta(theta);
setPsi(psi);
}
void AgentCore::dynamics() {
double theta = getTheta(pose_.orientation);
double x_dot_new = speed_command_sat_ * std::cos(theta);
double y_dot_new = speed_command_sat_ * std::sin(theta);
double theta_dot_new = speed_command_sat_ / vehicle_length_ * std::tan(steer_command_sat_);
geometry_msgs::Pose pose_old = pose_;
pose_.position.x = integrator(pose_.position.x, twist_.linear.x, x_dot_new, 1);
pose_.position.y = integrator(pose_.position.y, twist_.linear.y, y_dot_new, 1);
setTheta(pose_.orientation, integrator(theta, twist_.angular.z, theta_dot_new, 1));
twist_.linear.x = x_dot_new;
twist_.linear.y = y_dot_new;
twist_.angular.z = theta_dot_new;
std::stringstream s;
s << "Pose (" << pose_.position.x << ", " << pose_.position.y << ").";
console(__func__, s, DEBUG_VVV);
s << "Twist (" << twist_.linear.x << ", " << twist_.linear.y << ").";
console(__func__, s, DEBUG_VVV);
s << "System state (" << x_dot_new << ", " << y_dot_new << ", " << theta_dot_new << ").";
console(__func__, s, DEBUG_VVVV);
broadcastPose(pose_, agent_frame_);
broadcastPath(pose_, pose_old, agent_frame_);
}
EulerAngles::EulerAngles() :
Vector(3)
{
setPhi(0.0f);
setTheta(0.0f);
setPsi(0.0f);
}
void BlackScholesGreeks::setValues(double premium, double delta, double gamma, double theta, double vega, double rho)
{
setPremium(premium);
setDelta(delta);
setGamma(gamma);
setTheta(theta);
setVega(vega);
setRho(rho);
}
Eigen::MatrixXd GLMMBelief::evaluateSecondDerivative(const Eigen::VectorXd& x,
const Parameters& parameters)
{
setTheta(parameters.GLMMTheta);
setBeta(parameters.GLMMBeta);
auto family = parameters.GLMMFamily;
auto linearPredictor = computeLinearPredictor(x);
return LambdatThetaZt_ * family.evaluateSecondDerivative(linearPredictor, response_, weights_).matrix().asDiagonal() * LambdatThetaZt_.transpose();
}
double GLMMBelief::evaluate(const Eigen::VectorXd& x,
const Parameters& parameters)
{
setTheta(parameters.GLMMTheta);
setBeta(parameters.GLMMBeta);
auto family = parameters.GLMMFamily;
auto linearPredictor = computeLinearPredictor(x);
return family.evaluate(linearPredictor, response_, weights_);
}
void Perceptron::setRandomWeights(int num)
{
for(int i=0;i<num;i++)
{
setWeight((rand()%11)/10.0);
}
setTheta((rand()%11)/10.0);
//setTheta(1.0);
}
// Initialization of parameters from R
void PBC::parametersInit(SEXP m_binMat, SEXP m_theta, SEXP m_x, SEXP m_root, SEXP m_f, SEXP m_nIteration,
SEXP m_dxf, SEXP m_dxdyf, SEXP m_graf, SEXP m_gradxf, SEXP m_gradxdyf, SEXP m_type, SEXP m_g, SEXP m_dxg, SEXP m_out) {
setType(m_type);
setNIteration(m_nIteration);
setFunction(m_f);
setDxf(m_dxf);
setDxdyf(m_dxdyf);
setGraf(m_graf);
setGradxf(m_gradxf);
setGradxdyf(m_gradxdyf);
setVector(m_x);
setTheta(m_theta);
setRoot(m_root);
setBinMat(m_binMat, m_x);
setMargins(m_g);
setDxg(m_dxg);
setOut(m_out);
}
EulerAngles::EulerAngles(const Dcm &dcm) :
Vector(3)
{
setTheta(asinf(-dcm(2, 0)));
if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) {
setPhi(0.0f);
setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
dcm(0, 2) + dcm(1, 1)) + getPhi());
} else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) {
setPhi(0.0f);
setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
dcm(0, 2) + dcm(1, 1)) - getPhi());
} else {
setPhi(atan2f(dcm(2, 1), dcm(2, 2)));
setPsi(atan2f(dcm(1, 0), dcm(0, 0)));
}
}
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_);
}
void Position::rotate(double radians) {
setTheta(theta + radians);
}
Position::Position(double a,double b ,double c)
{
setX(a);setY(b);setTheta(c);
}
void Position::add(Position absolutePos) {
y += absolutePos.getY();
x += absolutePos.getX();
setTheta(theta + absolutePos.getTheta());
}
AgentCore::AgentCore() {
// handles server private parameters (private names are protected from accidental name collisions)
private_node_handle_ = new ros::NodeHandle("~");
private_node_handle_->param("sample_time", sample_time_, (double)DEFAULT_SAMPLE_TIME);
private_node_handle_->param("slot_tdma", slot_tdma_, (double)DEFAULT_SLOT_TDMA);
private_node_handle_->param("agent_id", agent_id_, DEFAULT_AGENT_ID);
private_node_handle_->param("number_of_stats", number_of_stats_, DEFAULT_NUMBER_OF_STATS);
private_node_handle_->param("number_of_velocities", number_of_velocities_, DEFAULT_NUMBER_OF_VELOCITIES);
private_node_handle_->param("verbosity_level", verbosity_level_, DEFAULT_VERBOSITY_LEVEL);
private_node_handle_->param("velocity_virtual_threshold", velocity_virtual_threshold_, (double)DEFAULT_VELOCITY_VIRTUAL_THRESHOLD);
private_node_handle_->param("speed_min", speed_min_, (double)DEFAULT_SPEED_MIN);
private_node_handle_->param("speed_max", speed_max_, (double)DEFAULT_SPEED_MAX);
private_node_handle_->param("steer_min", steer_min_, (double)DEFAULT_STEER_MIN);
private_node_handle_->param("steer_max", steer_max_, (double)DEFAULT_STEER_MAX);
private_node_handle_->param("k_p_speed", k_p_speed_, (double)DEFAULT_K_P_SPEED);
private_node_handle_->param("k_p_steer", k_p_steer_, (double)DEFAULT_K_P_STEER);
private_node_handle_->param("vehicle_length", vehicle_length_, (double)DEFAULT_VEHICLE_LENGTH);
private_node_handle_->param("world_limit", world_limit_, (double)DEFAULT_WORLD_LIMIT);
const std::vector<double> DEFAULT_DIAG_ELEMENTS_GAMMA = {100, 100, 5, 10, 5};
const std::vector<double> DEFAULT_DIAG_ELEMENTS_LAMBDA = {0, 0, 0, 0, 0};
const std::vector<double> DEFAULT_DIAG_ELEMENTS_B = {100, 100};
std::vector<double> diag_elements_gamma;
std::vector<double> diag_elements_lambda;
std::vector<double> diag_elements_b;
private_node_handle_->param("diag_elements_gamma", diag_elements_gamma, DEFAULT_DIAG_ELEMENTS_GAMMA);
private_node_handle_->param("diag_elements_lambda", diag_elements_lambda, DEFAULT_DIAG_ELEMENTS_LAMBDA);
private_node_handle_->param("diag_elements_b", diag_elements_b, DEFAULT_DIAG_ELEMENTS_B);
gamma_ = Eigen::Map<Eigen::VectorXd>(diag_elements_gamma.data(), number_of_stats_).asDiagonal();
lambda_ = Eigen::Map<Eigen::VectorXd>(diag_elements_lambda.data(), number_of_stats_).asDiagonal();
b_ = Eigen::Map<Eigen::VectorXd>(diag_elements_b.data(), number_of_velocities_).asDiagonal();
jacob_phi_ = Eigen::MatrixXd::Identity(number_of_stats_, number_of_velocities_);
phi_dot_.resize(number_of_stats_);
std::random_device rd;
std::mt19937 generator(rd());
std::uniform_real_distribution<> distrib_position(-world_limit_, world_limit_);
std::uniform_real_distribution<> distrib_orientation(-M_PI, M_PI);
double theta;
// agent pose initialization (we assume a null twist at the beginning)
private_node_handle_->param("x", pose_.position.x, distrib_position(generator));
private_node_handle_->param("y", pose_.position.y, distrib_position(generator));
private_node_handle_->param("theta", theta, distrib_orientation(generator));
pose_.orientation.w = 1;
setTheta(pose_.orientation, theta);
pose_virtual_ = pose_;
std::vector<double> initial_estimation = {pose_.position.x, pose_.position.y, std::pow(pose_.position.x, 2),
pose_.position.x * pose_.position.y, std::pow(pose_.position.y, 2)};
estimated_statistics_ = statsVectorToMsg(initial_estimation);
target_statistics_ = estimated_statistics_;
private_node_handle_->param("topic_queue_length", topic_queue_length_, DEFAULT_TOPIC_QUEUE_LENGTH);
private_node_handle_->param("shared_stats_topic", shared_stats_topic_name_, std::string(DEFAULT_SHARED_STATS_TOPIC));
private_node_handle_->param("target_stats_topic", target_stats_topic_name_, std::string(DEFAULT_TARGET_STATS_TOPIC));
private_node_handle_->param("marker_topic", marker_topic_name_, std::string(DEFAULT_MARKER_TOPIC));
private_node_handle_->param("marker_path_lifetime", marker_path_lifetime_, DEFAULT_MARKER_PATH_LIFETIME);
private_node_handle_->param("enable_path", enable_path_, true);
private_node_handle_->param("frame_map", frame_map_, std::string(DEFAULT_FRAME_MAP));
private_node_handle_->param("frame_agent_prefix", frame_agent_prefix_, std::string(DEFAULT_FRAME_AGENT_PREFIX));
private_node_handle_->param("frame_virtual_suffix", frame_virtual_suffix_, std::string(DEFAULT_FRAME_VIRTUAL_SUFFIX));
agent_frame_ = frame_agent_prefix_ + std::to_string(agent_id_);
agent_virtual_frame_ = agent_frame_ + frame_virtual_suffix_;
stats_publisher_ = node_handle_.advertise<formation_control::FormationStatisticsStamped>(shared_stats_topic_name_, topic_queue_length_);
stats_subscriber_ = node_handle_.subscribe(shared_stats_topic_name_, topic_queue_length_, &AgentCore::receivedStatsCallback, this);
target_stats_subscriber_ = node_handle_.subscribe(target_stats_topic_name_, topic_queue_length_, &AgentCore::targetStatsCallback, this);
marker_publisher_ = node_handle_.advertise<visualization_msgs::Marker>(marker_topic_name_, topic_queue_length_);
waitForSlotTDMA(sample_time_); // sync to the next sample time slot
// must be immediately after the waitForSlotTDMA method to ensure a satisfactory synchronization with TDMA protocol
algorithm_timer_ = private_node_handle_->createTimer(ros::Duration(sample_time_), &AgentCore::algorithmCallback, this);
}
void Position::sub(Position absolutePos) {
y -= absolutePos.getY();
x -= absolutePos.getX();
setTheta(theta - absolutePos.getTheta());
}
void Position::moveAbsolute(Move move) {
y += move.getY();
x += move.getX();
setTheta(theta + move.getTheta());
}
void Position::moveRelative(Move move) {
y += move.getX() * sin(theta) + move.getY() * cos(theta);
x += move.getX() * cos(theta) - move.getY() * sin(theta);
setTheta(theta + move.getTheta());
}
CCLib::SimpleCloud* ccGBLSensor::project(CCLib::GenericCloud* theCloud, int& errorCode, bool autoParameters/*false*/)
{
assert(theCloud);
CCLib::SimpleCloud* newCloud = new CCLib::SimpleCloud();
unsigned pointCount = theCloud->size();
if (!newCloud->reserve(pointCount) || !newCloud->enableScalarField()) //not enough memory
{
errorCode = -4;
delete newCloud;
return 0;
}
ScalarType maxDepth = 0;
theCloud->placeIteratorAtBegining();
{
for (unsigned i=0; i<pointCount; ++i)
{
const CCVector3 *P = theCloud->getNextPoint();
CCVector2 Q;
ScalarType depth;
projectPoint(*P,Q,depth);
newCloud->addPoint(CCVector3(Q.x,Q.y,0));
newCloud->setPointScalarValue(i,depth);
if (i != 0)
maxDepth = std::max(maxDepth,depth);
else
maxDepth = depth;
}
}
if (autoParameters)
{
//dimensions = theta, phi, 0
PointCoordinateType bbMin[3],bbMax[3];
newCloud->getBoundingBox(bbMin,bbMax);
setTheta(bbMin[0],bbMax[0]);
setPhi(bbMin[1],bbMax[1]);
setSensorRange(maxDepth);
}
//clear previous Z-buffer
{
if (m_depthBuffer.zBuff)
delete[] m_depthBuffer.zBuff;
m_depthBuffer.zBuff=0;
m_depthBuffer.width=0;
m_depthBuffer.height=0;
}
//init new Z-buffer
{
int width = static_cast<int>(ceil((thetaMax-thetaMin)/deltaTheta));
int height = static_cast<int>(ceil((phiMax-phiMin)/deltaPhi));
if (width*height == 0 || std::max(width,height) > 2048) //too small or... too big!
{
errorCode = -2;
delete newCloud;
return 0;
}
unsigned zBuffSize = width*height;
m_depthBuffer.zBuff = new ScalarType[zBuffSize];
if (!m_depthBuffer.zBuff) //not enough memory
{
errorCode = -4;
delete newCloud;
return 0;
}
m_depthBuffer.width = width;
m_depthBuffer.height = height;
memset(m_depthBuffer.zBuff,0,zBuffSize*sizeof(ScalarType));
}
//project points and accumulate them in Z-buffer
newCloud->placeIteratorAtBegining();
for (unsigned i=0; i<newCloud->size(); ++i)
{
const CCVector3 *P = newCloud->getNextPoint();
ScalarType depth = newCloud->getPointScalarValue(i);
int x = static_cast<int>(floor((P->x-thetaMin)/deltaTheta));
int y = static_cast<int>(floor((P->y-phiMin)/deltaPhi));
ScalarType& zBuf = m_depthBuffer.zBuff[y*m_depthBuffer.width+x];
zBuf = std::max(zBuf,depth);
}
errorCode = 0;
return newCloud;
}
