std::vector<float> EyeRover::prepare_sensors_data(double values[]){
std::vector<float> value_vector(3);
//Version 2 of the eyeRover has a different layout of the board
//We need to swap y and z axis and z should be inverted
if (m_version == 1){
value_vector[0] = values[0];
value_vector[1] = values[1];
value_vector[2] = values[2];
}else{ // m_version == 2
value_vector[0] = values[0];
value_vector[1] = -values[2];
value_vector[2] = values[1];
}
return value_vector;
}
void baxter_sim_application_t::randomize_positions()
{
PRX_ERROR_S("\n\n\n SIMULATOR STATE SPACE ::: "<<simulator->get_state_space()->print_memory(4));
PRX_ERROR_S(" SIMULATOR STATE SPACE SET TO::: "<<simulator->get_state_space()->print_point(simulator_state,4));
std::vector<movable_body_plant_t* > objects;
manipulation_simulator_t* manip_sim = dynamic_cast<manipulation_simulator_t* >(simulator);
// AI_PRX_TODO : Randomize the position of the objects.
// First obtain the list of movable objects from the manipulator simulator.
// std::vector<movable_body_plant_t* > objects;
manip_sim->get_movable_objects(objects);
// Then get the state space of each object
std::vector<const space_t*> objects_spaces;
for (int i = 0; i < objects.size(); ++i){
objects_spaces.push_back(objects[i]->get_state_space());
}
// Then update the state space of each with a point/vector that you change here randomly within the following range
// simulator_state ==>> spaces point typer for the simulator
// what we need is a space point type for the object space
// allocate one point and everytime randomly re-assign value to the objects' spaces
// RANGE OF POSITIONS:
// X belongs to [0.75, 0.95]
// Y belongs to [0.40, 0.60]
// Z = 0.87
// Q = <0 0 0 1>
space_point_t* object_point = objects_spaces[0]->alloc_point();
double value_array[7] = {0.85, 0.50, 0.87, 0, 0, 0, 1};
std::vector<double> value_vector(value_array, value_array+sizeof(value_array)/sizeof(double));
objects_spaces[0]->set_from_vector(value_vector, object_point);
// copy new object_point into the object space
objects_spaces[0]->copy_from_point(object_point);
// After you update every object,
// Get the full state space and copy to simulator_state
/*
When you are dealing with the manipulation simulator, there are additional internal state variables the simulator uses for grasping properly.
These might not be set correctly if you only do copy_from_point, since that can update only part of the state space while the rest is unchanged.
push_state is done by the application to both copy the state values and also update the manipulation simulator information.
You do not need to do this unless you are at the application level.
*/
simulator->push_state(simulator_state);//By default the objects return to their original position
PRX_ERROR_S("SIMULATOR STATE SPACE CHANGED TO::: "<<simulator->get_state_space()->print_memory(4));
}