void WalkParameterWidget::onSendRequested(string text)
{
debug << "sending" << endl;
JobList jobs;
WalkParameters parameters;
stringstream ss(text);
ss >> parameters;
jobs.addMotionJob(new WalkParametersJob(parameters));
*nuio << jobs;
debug << parameters;
}
/*! @brief Jobs created by the WalkOptimiserBehaviour are added here.
The behaviour has 4 states:
1. Initial where we are just waiting for the simulator to go into 'playing'
3. MeasureCost where we actually perform the measurement of the cost
5. MeasureRobust where we actually perform the measurement of the robustness
6. Teleport where the robot is teleported back to its initial position
@param joblist the list of jobs to which job(s) will be added
*/
void WalkOptimiserBehaviour::process(JobList& joblist)
{
static int fallencount = 0;
static vector<float> speed(3,0);
static WalkJob* job = new WalkJob(speed);
if (m_data == NULL || m_actions == NULL)
return;
m_previous_state = m_state;
m_previous_time = m_current_time;
m_current_time = m_data->CurrentTime;
if (m_state == Initial)
{ // In the initial state we wait until the simulator puts the game in 'playing' and I have time to score a goal!
if (m_data->CurrentTime > 15000)
{
m_state = MeasureCost;
teleport();
}
}
else if (m_state == Teleport)
{ // In the teleport state we wait until the robot has stopped
// We then respawn, and then wait another 0.5s before proceeding to the next state
// handle the deacceleration (now done in the walk engine itself)
m_target_speed = 0;
// wait until the robot comes to rest
static vector<float> speed(3,0);
static double stoppedtime = 0;
static bool stopped = false;
m_walk->getCurrentSpeed(speed);
if (stopped == false && speed[0] == 0)
{
stoppedtime = m_current_time;
stopped = true;
}
// handle the respawn call (being careful to only call it once because it really slows webots down)
static bool respawn_called = false;
if (respawn_called == false && stopped == true && (m_current_time - stoppedtime) > 500)
{
respawn();
respawn_called = true;
}
// handle the timely progress to the next state
if (respawn_called == true && stopped == true && (m_current_time - stoppedtime) > 1000)
{
m_state = m_next_state;
m_target_speed = 0;
fallencount = 0;
respawn_called = false;
stopped = false;
if (m_trial_out_of_field == false)
{
if (m_state == MeasureCost)
startCostTrial();
else if (m_state == MeasureRobust)
startRobustTrial();
}
else
m_trial_out_of_field = false;
}
}
else if (m_data->isFallen())
{ // In the fallen state, we wait to make sure we are actually fallen, and then finish the current trial
fallencount++;
if (fallencount > 9)
{
if (m_state == MeasureCost)
finishMeasureCost();
else if (m_state == MeasureRobust)
finishMeasureRobust();
}
}
else
{
fallencount = 0; // reset the fallen count when we are not falling
// handle accelerating walks up to maximum speed (now done in the walk engine itself)
m_target_speed = m_walk_parameters[0];
// look for terminating distances while measuring walk performance
static vector<float> gps(3,0);
m_data->getGPSValues(gps);
float totaldistance = sqrt(pow(gps[0] - m_respawn_x,2) + pow(gps[1] - m_respawn_y,2));
if (m_state == MeasureCost)
{
if (totaldistance > 300.0) // if walked more than 300cm begin to stop the robot
{
m_target_speed = 0;
static vector<float> speeds(3,0);
m_walk->getCurrentSpeed(speeds);
if (speeds[0] == 0) // once the robot has stopped finish the cost measurement
finishMeasureCost();
}
}
else if (m_state == MeasureRobust)
{
if (totaldistance > 450.0)
{
m_trial_out_of_field = true;
teleport();
}
}
}
speed[0] = m_target_speed;
job->setSpeed(speed);
joblist.addMotionJob(job);
}
