Here are the compiler flags I use when compiling and linking (for gcc7):
Code: Select all
-O3 -ggdb -Ofast -fno-trapping-math -fno-signed-zeros -march=native -fno-stack-protector -flto
I'm using the footbot proximity, light, motor_ground sensors (though there are others in the input .argos file that I don't use that are used by other controllers--does that matter?).
Here is the ControlStep() of the controller I'm using:
Code: Select all
void crw_controller::ControlStep(void) {
saa_subsystem()->actuation()->block_carry_throttle(is_carrying_block());
saa_subsystem()->actuation()->throttling_update(
saa_subsystem()->sensing()->tick());
m_fsm->run();
} /* ControlStep() */
The controller basically just runs a state machine (the m_fsm->run() call), which just executes a correlated random walk behavior:
Code: Select all
void random_explore_behavior::execute(void) {
rmath::vector2d obs = saa_subsystem()->sensing()->find_closest_obstacle();
saa_subsystem()->steering_force().avoidance(obs);
saa_subsystem()->steering_force().wander();
if (saa_subsystem()->sensing()->threatening_obstacle_exists()) {
saa_subsystem()->apply_steering_force(std::make_pair(false, false));
saa_subsystem()->actuation()->leds_set_color(utils::color::kRED);
} else {
saa_subsystem()->actuation()->leds_set_color(utils::color::kMAGENTA);
rmath::vector2d force = saa_subsystem()->steering_force().value();
}
The steering force for obstacle avoidance and wandering are calculated as:
Code: Select all
math::vector2d avoidance_force::operator()(const boid &,
const math::vector2d &closest) const {
if (closest.length() > 0) {
math::vector2d avoidance = -closest;
return avoidance.normalize() * mc_max;
} else {
return {0, 0}; /* no threatening obstacles = no avoidance */
}
} /* operator()() */
Code: Select all
/*
* Only actually apply the wander force at the specified cadence. Otherwise
* random perturbations between [-n, n] will sum to 0 (no net wandering) over
* time if the perturbations are applied every timestep.
*/
++m_count;
if (m_count % m_interval != 0) {
return {0, 0};
}
/* calculate circle center */
math::vector2d velocity;
if (entity.linear_velocity().length() <= 0) {
velocity = math::vector2d(1, 0);
} else {
velocity = entity.linear_velocity();
}
math::vector2d circle_center = (velocity).normalize().scale(m_circle_distance);
/* calculate displacement force (the actual wandering) */
math::vector2d displacement(
m_circle_radius * std::cos((m_angle + velocity.angle()).value()),
m_circle_radius * std::sin((m_angle + velocity.angle()).value()));
double val;
if (m_use_normal) {
val = m_normal_dist(m_rng);
} else {
val = -m_max_angle_delta +
2 * m_max_angle_delta * (static_cast<double>(std::rand()) / RAND_MAX);
}
math::degrees perturbation(std::fmod(math::to_degrees(m_angle).value() + val,
m_max_angle_delta));
m_angle = math::to_radians(perturbation);
/*
* Wandering is a combination of the current velocity, projected outward a
* certain distance, and a displacement calculated at that point.
*
* There can be discontinuous jumps from a small positive angle for the wander
* force to a large negative angle between timesteps and vice versa which play
* havoc with robots' ability to compensate.
*
* So, compute the angle indirectly using sine and cosine in order to account
* for sign differences and ensure continuity.
*/
double angle = std::atan2(displacement.y() - circle_center.y(),
displacement.x() - circle_center.x());
double angle_diff = angle - circle_center.angle().value();
angle_diff = std::atan2(std::sin(angle_diff), std::cos(angle_diff));
if (std::fabs(angle_diff - m_last_angle) > M_PI) {
angle_diff -= std::copysign(2 * M_PI, angle_diff);
}
m_last_angle = angle_diff;
math::vector2d wander((circle_center + displacement).length(),
math::radians(angle_diff));
return wander.normalize() * m_max;
Lastly, here is the input file I'm using (project-specific parameters omitted for clarity):
Code: Select all
<argos-configuration>
<framework>
<system threads="16" />
<experiment length="5000" random_seed="33" ticks_per_second="5" />
</framework>
<controllers>
<crw_controller id="ffc" library="libfordyca">
<actuators>
<differential_steering implementation="default" />
<leds implementation="default" medium="leds" />
<range_and_bearing implementation="default" />
</actuators>
<sensors>
<footbot_proximity implementation="default" show_rays="true" />
<footbot_light implementation="rot_z_only" show_rays="false" />
<footbot_motor_ground implementation="rot_z_only" />
<range_and_bearing implementation="medium" medium="rab" />
<battery implementation="default" />
</sensors>
<params>
</params>
</crw_controller>
</controllers>
<loop_functions label="depth0_loop_functions" library="libfordyca">
</loop_functions>
<arena center="16.0, 8.0, 1" size="32, 16, 2">
<floor id="floor" pixels_per_meter="50" source="loop_functions" />
<box id="wall_north" movable="false" size="32, 0.1, 0.5">
<body orientation="0,0,0" position="16.0, 16, 0" />
</box>
<box id="wall_south" movable="false" size="32, 0.1, 0.5">
<body orientation="0,0,0" position="16.0, 0, 0 " />
</box>
<box id="wall_east" movable="false" size="0.1, 16, 0.5">
<body orientation="0,0,0" position="32, 8.0, 0" />
</box>
<box id="wall_west" movable="false" size="0.1, 16, 0.5">
<body orientation="0,0,0" position="0, 8.0, 0" />
</box>
<distribute>
<position max="30, 15, 0" method="uniform" min="1,1,0" />
<orientation max="360,0,0" method="uniform" min="0,0,0" />
<entity max_trials="1000" quantity="1024">
<foot-bot id="fb">
<controller config="ffc" />
</foot-bot>
</entity>
</distribute>
</arena>
<media>
<range_and_bearing id="rab" />
<led id="leds" />
</media>
<physics_engines>
<dynamics2d id="dyn2d0">
<boundaries>
<top height="1" />
<bottom height="0" />
<sides>
<vertex point="0.0, 0.0" />
<vertex point="8.0, 0.0" />
<vertex point="8.0, 4.0" />
<vertex point="0.0, 4.0" />
</sides>
</boundaries>
</dynamics2d>
<dynamics2d id="dyn2d1">
<boundaries>
<top height="1" />
<bottom height="0" />
<sides>
<vertex point="8.0, 0.0" />
<vertex point="16.0, 0.0" />
<vertex point="16.0, 4.0" />
<vertex point="8.0, 4.0" />
</sides>
</boundaries>
</dynamics2d>
<dynamics2d id="dyn2d2">
<boundaries>
<top height="1" />
<bottom height="0" />
<sides>
<vertex point="16.0, 0.0" />
<vertex point="24.0, 0.0" />
<vertex point="24.0, 4.0" />
<vertex point="16.0, 4.0" />
</sides>
</boundaries>
</dynamics2d>
<dynamics2d id="dyn2d3">
<boundaries>
<top height="1" />
<bottom height="0" />
<sides>
<vertex point="24.0, 0.0" />
<vertex point="32.0, 0.0" />
<vertex point="32.0, 4.0" />
<vertex point="24.0, 4.0" />
</sides>
</boundaries>
</dynamics2d>
<dynamics2d id="dyn2d4">
<boundaries>
<top height="1" />
<bottom height="0" />
<sides>
<vertex point="24.0, 4.0" />
<vertex point="32.0, 4.0" />
<vertex point="32.0, 8.0" />
<vertex point="24.0, 8.0" />
</sides>
</boundaries>
</dynamics2d>
<dynamics2d id="dyn2d5">
<boundaries>
<top height="1" />
<bottom height="0" />
<sides>
<vertex point="16.0, 4.0" />
<vertex point="24.0, 4.0" />
<vertex point="24.0, 8.0" />
<vertex point="16.0, 8.0" />
</sides>
</boundaries>
</dynamics2d>
<dynamics2d id="dyn2d6">
<boundaries>
<top height="1" />
<bottom height="0" />
<sides>
<vertex point="8.0, 4.0" />
<vertex point="16.0, 4.0" />
<vertex point="16.0, 8.0" />
<vertex point="8.0, 8.0" />
</sides>
</boundaries>
</dynamics2d>
<dynamics2d id="dyn2d7">
<boundaries>
<top height="1" />
<bottom height="0" />
<sides>
<vertex point="0.0, 4.0" />
<vertex point="8.0, 4.0" />
<vertex point="8.0, 8.0" />
<vertex point="0.0, 8.0" />
</sides>
</boundaries>
</dynamics2d>
<dynamics2d id="dyn2d8">
<boundaries>
<top height="1" />
<bottom height="0" />
<sides>
<vertex point="0.0, 8.0" />
<vertex point="8.0, 8.0" />
<vertex point="8.0, 12.0" />
<vertex point="0.0, 12.0" />
</sides>
</boundaries>
</dynamics2d>
<dynamics2d id="dyn2d9">
<boundaries>
<top height="1" />
<bottom height="0" />
<sides>
<vertex point="8.0, 8.0" />
<vertex point="16.0, 8.0" />
<vertex point="16.0, 12.0" />
<vertex point="8.0, 12.0" />
</sides>
</boundaries>
</dynamics2d>
<dynamics2d id="dyn2d10">
<boundaries>
<top height="1" />
<bottom height="0" />
<sides>
<vertex point="16.0, 8.0" />
<vertex point="24.0, 8.0" />
<vertex point="24.0, 12.0" />
<vertex point="16.0, 12.0" />
</sides>
</boundaries>
</dynamics2d>
<dynamics2d id="dyn2d11">
<boundaries>
<top height="1" />
<bottom height="0" />
<sides>
<vertex point="24.0, 8.0" />
<vertex point="32.0, 8.0" />
<vertex point="32.0, 12.0" />
<vertex point="24.0, 12.0" />
</sides>
</boundaries>
</dynamics2d>
<dynamics2d id="dyn2d12">
<boundaries>
<top height="1" />
<bottom height="0" />
<sides>
<vertex point="24.0, 12.0" />
<vertex point="32.0, 12.0" />
<vertex point="32.0, 16.0" />
<vertex point="24.0, 16.0" />
</sides>
</boundaries>
</dynamics2d>
<dynamics2d id="dyn2d13">
<boundaries>
<top height="1" />
<bottom height="0" />
<sides>
<vertex point="16.0, 12.0" />
<vertex point="24.0, 12.0" />
<vertex point="24.0, 16.0" />
<vertex point="16.0, 16.0" />
</sides>
</boundaries>
</dynamics2d>
<dynamics2d id="dyn2d14">
<boundaries>
<top height="1" />
<bottom height="0" />
<sides>
<vertex point="8.0, 12.0" />
<vertex point="16.0, 12.0" />
<vertex point="16.0, 16.0" />
<vertex point="8.0, 16.0" />
</sides>
</boundaries>
</dynamics2d>
<dynamics2d id="dyn2d15">
<boundaries>
<top height="1" />
<bottom height="0" />
<sides>
<vertex point="0.0, 12.0" />
<vertex point="8.0, 12.0" />
<vertex point="8.0, 16.0" />
<vertex point="0.0, 16.0" />
</sides>
</boundaries>
</dynamics2d>
</physics_engines>
</argos-configuration>
As far as locking threads to cores, there is not a POSIX/standard way to do it, but on linux you can use pthread_setaffinity_np() to set which CPUs a thread is eligible to run on, and sched_affinitity() to set which cores a process is eligible to run on (there are different calls to do it on OSX). Providing options for one or both of these in the .argos input file, or exposing the std::thread handle (at least I assumed that's what ARGoS uses to create/manage threads) so that users can set them programmatically would definitely be a great feature to add. On shared systems where ARGoS may have to contend with other high performance software as it runs this would help to make it as efficient as possible by minimizing overhead due to thread context switching/cache invalidation when threads switch cores.
I think this is something relatively simple that could be added, and I would like contribute to ARGoS and add it. How can I get started on doing so?
- John