void simple_controller_t::add_system(const std::string& path, system_ptr_t system)
{
std::string name;
std::string subpath;
boost::tie(name, subpath) = split_path(path);
PRX_DEBUG_S("+++ ADD system : name : " << name << " subpath: " << subpath);
if( name.empty() )
throw invalid_path_exception("Name is empty");
else if( subpath.empty() )
{
PRX_ERROR_S("Trying to add system to a simple controller. Throwing exception...");
throw simple_add_remove_exception();
}
else if( !subpath.empty() )
{
if( subsystems.find(name) != subsystems.end() )
subsystems[name]->add_system(subpath, system);
}
else
throw invalid_path_exception(path);
construct_spaces();
verify();
}
void simple_controller_t::remove_system(const std::string& path)
{
std::string name;
std::string subpath;
boost::tie(name, subpath) = split_path(path);
PRX_DEBUG_S("--- REMOVE system : name : " << name << " subpath : " << subpath);
if( subsystems.find(name) != subsystems.end() )
if( subpath.empty() )
{
PRX_ERROR_S("Trying to remove system from a simple controller. Throwing exception...");
throw simple_add_remove_exception();
}
else
{
try
{
subsystems[name]->remove_system(subpath);
}
catch( removal_exception e )
{
subsystems.erase(name);
}
}
else
throw invalid_path_exception("The system " + name + " does not exist in the path " + pathname);
construct_spaces();
verify();
}
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));
}
void simple_controller_t::verify() const
{
// Check our spaces to make sure they exist
// PRX_ASSERT(input_control_space != NULL);
PRX_ASSERT(output_control_space != NULL);
PRX_ASSERT(state_space != NULL);
// Simple controllers must have one subsystem
PRX_ASSERT(subsystems.size() == 1);
// Verify validity of our spaces
if( input_control_space != NULL )
input_control_space->verify();
output_control_space->verify();
state_space->verify();
if( controller_state_space )
controller_state_space->verify();
hash_t<std::string, system_ptr_t >::const_iterator subsystems_const_iter;
// Call our subsystems verify
subsystems.begin()->second->verify();
// Check to make sure our output_control_space == subsytems->get_control_space()
std::string space_name;
const space_t* cspace;
cspace = subsystems.begin()->second->get_control_space();
if( cspace != NULL )
space_name = cspace->get_space_name();
// Throw an exception if we do not match control spaces
if( std::strcmp(space_name.c_str(), output_control_space->get_space_name().c_str()) )
{
PRX_ERROR_S("Output control space does not match subsystem's input control space");
throw space_mismatch_exception();
}
}
bool motion_planner_t::deserialize()
{
PRX_ERROR_S("Deserialize called on motion planner: Empty implementation!");
return false;
}
