const std::string parameter_reader_t::get_value(const std::string& key) const
{
std::string value;
try
{
if( root.getType() != XmlRpc::XmlRpcValue::TypeStruct )
{
if( key.empty() )
value = get_xmlrpc_as_string(root);
else
PRX_WARN_S("Expected dictionary in " << trace() << " for key " << key);
}
else
{
if( has_key(key) )
value = get_xmlrpc_as_string(root[key]);
else
PRX_WARN_S("Expected key " << key << " in " << trace());
}
}
catch( XmlRpc::XmlRpcException e )
{
PRX_FATAL_S("Exception (" << e.getMessage() << ")"
<< " reading ROS parameter server value: "
<< path << '/' << key);
}
return value;
}
void apc_sampler_t::init(const parameter_reader_t* reader, const parameter_reader_t* template_reader)
{
theta_error = parameters::get_attribute_as<int>("theta_error", reader, template_reader, 10);
max_tries = parameters::get_attribute_as<int>("max_tries", reader, template_reader, 10);
neighs = parameters::get_attribute_as<int>("neighbors", reader, template_reader, 50);
std::string mode = parameters::get_attribute("mode", reader, template_reader, "transit");
if( mode == "transit" )
{
gripper_mode = GRIPPER_OPEN;
}
else if( mode == "transfer" )
{
gripper_mode = GRIPPER_CLOSED;
}
else
{
PRX_WARN_S("Invalid sampling mode for apc_sampler_t!\n Transfer mode will be used.");
gripper_mode = GRIPPER_CLOSED;
}
//Shouldn't we read this in from input or something maybe?
relative_configuration.set_position(0, 0, 0);
relative_configuration.set_orientation(0, 0.707106, 0, 0.707106);
}
std::auto_ptr<const parameter_reader_t> parameter_reader_t::get_child(const std::string& path) const
{
std::string name;
std::string subpath;
boost::tie(name, subpath) = split_path(path);
if( path.empty() )
{
PRX_FATAL_S("Calling get child with empty path");
}
else if( subpath.empty() ) // No slash found
{
return std::auto_ptr<const parameter_reader_t > (get_subreader(path));
}
else
{
if( !has_element(name) )
PRX_WARN_S("Can't follow path \"" << path << "\" from " << trace());
const parameter_reader_t* subreader = get_subreader(name);
std::auto_ptr<const parameter_reader_t> child_reader = subreader->get_child(subpath);
delete subreader;
return child_reader;
}
}
bool geometry_t::contains_point(const vector_t& v, double buffer)
{
double squared_rad;
vector_t n;
switch(type)
{
case PRX_SPHERE://sphere
squared_rad = info[0]*info[0];
if(v.squared_norm() < squared_rad+buffer)
return true;
break;
case PRX_BOX://box
if(v[0]>-.5*info[0]-buffer && v[0]<.5*info[0]+buffer &&
v[1]>-.5*info[1]-buffer && v[1]<.5*info[1]+buffer &&
v[2]>-.5*info[2]-buffer && v[2]<.5*info[2]+buffer)
{
return true;
}
break;
case PRX_CONE:
PRX_WARN_S("Cannot check point containment in cone");
break;
case PRX_CYLINDER://cylinder
case PRX_OPEN_CYLINDER:
n = v;
n[2] = 0;
squared_rad = info[0]*info[0]+buffer;
if(v.squared_norm() < squared_rad &&
v[2]>-.5*info[1]-buffer && v[2]<.5*info[1]+buffer )
return true;
break;
case PRX_CAPSULE:
PRX_WARN_S("Cannot check point containment in capsule");
break;
case PRX_MESH:
// PRX_WARN_S(trimesh.min_x<<" "<<trimesh.max_x<<" "<<trimesh.min_y<<" "<<trimesh.max_y<<" "<<trimesh.min_z<<" "<<trimesh.max_z);
if( trimesh.min_x - buffer < v[0] && trimesh.max_x + buffer > v[0]
&& trimesh.min_y - buffer < v[1] && trimesh.max_y + buffer > v[1]
&& trimesh.min_z - buffer < v[2] && trimesh.max_z + buffer > v[2])
return true;
break;
default:
PRX_WARN_S("Cannot check point containment in geometry");
}
return false;
}
void minimal_avoidance_controller_t::compute_control()
{
/** Check proximity info */
if (prox_info->get_closest_obstacle_distance() < obstacle_distance || prox_info->get_closest_plant_distance() < plant_distance)
{
output_control_space->copy_from_point(contingency_plan.get_control_at(0));
PRX_WARN_S ("Executing safety control: " << output_control_space->print_memory());
}
subsystems.begin()->second->compute_control();
}
void goal_state_t::init(const parameter_reader_t* reader, const parameter_reader_t* template_reader)
{
goal_t::init(reader, template_reader);
if( parameters::has_attribute("goal_state", reader, template_reader) )
{
state_vec = parameters::get_attribute_as< std::vector<double> >("goal_state", reader, template_reader);
}
else
{
PRX_WARN_S("Missing goal_state attribute in input files.");
}
}
void insert(astar_node_t* element)
{
if( finder.find(element) != finder.end() )
{
PRX_WARN_S("Element already exists in open set, should be using update instead.");
return;
}
// PRX_DEBUG_COLOR("Heap size: " << heap.size() << " heap_size: " << heap_size, PRX_TEXT_LIGHTGRAY);
if( heap.size() > heap_size )
{
heap[heap_size] = element;
}
else
{
heap.push_back(element);
}
heap_size++;
finder[element] = heap_size - 1;
reheap_up(heap_size - 1);
}
std::vector<const parameter_reader_t*> parameter_reader_t::get_list(const std::string& path) const
{
std::string name;
std::string subpath;
boost::tie(name, subpath) = split_path(path);
if( subpath.empty() ) // No slash found
{
return get_subreaders(path);
}
else
{
if( !has_element(name) )
PRX_WARN_S("Can't follow path \"" << path << "\" from " << trace());
const parameter_reader_t* subreader = get_subreader(name);
std::vector<const parameter_reader_t*> subreaders = subreader->get_list(subpath);
delete subreader;
return subreaders;
}
}
int arc_t::intersection( const arc_t* other, std::vector< intersection_point_t >& ret ) const
{
check_calc();
// PRX_LOG_DEBUG ("Inside arc intersection");
// The two circles are |X-C0| = R0 and |X-C1| = R1. Define U = C1 - C0
// and V = Perp(U) where Perp(x,y) = (y,-x). Note that Dot(U,V) = 0 and
// |V|^2 = |U|^2. The intersection points X can be written in the form
// X = C0+s*U+t*V and X = C1+(s-1)*U+t*V. Squaring the circle equations
// and substituting these formulas into them yields
// R0^2 = (s^2 + t^2)*|U|^2
// R1^2 = ((s-1)^2 + t^2)*|U|^2.
// Subtracting and solving for s yields
// s = ((R0^2-R1^2)/|U|^2 + 1)/2
// Then replace in the first equation and solve for t^2
// t^2 = (R0^2/|U|^2) - s^2.
// In order for there to be solutions, the right-hand side must be
// nonnegative. Some algebra leads to the condition for existence of
// solutions,
// (|U|^2 - (R0+R1)^2)*(|U|^2 - (R0-R1)^2) <= 0.
// This reduces to
// |R0-R1| <= |U| <= |R0+R1|.
// If |U| = |R0-R1|, then the circles are side-by-side and just tangent.
// If |U| = |R0+R1|, then the circles are nested and just tangent.
// If |R0-R1| < |U| < |R0+R1|, then the two circles to intersect in two
// points.
vector_t U = other->get_point_c()->get_data() - center.get_data();
double USqrLen = U.squared_norm();
double R0 = this->rad, R1 = other->get_rad();
double R0mR1 = R0 - R1;
// PRX_LOG_DEBUG ("U vector is");
// U.print();
// PRX_LOG_DEBUG (" Squared Length is %f", USqrLen);
// PRX_LOG_DEBUG ( " R0: %f, R1: %f, R0-R1: %f", R0, R1, R0mR1);
if (USqrLen < PRX_ZERO_CHECK && std::fabs(R0mR1) < PRX_ZERO_CHECK)
{
// Circles are essentially the same.
ret[0].set_valid( false );
ret[1].set_valid( false );
PRX_WARN_S ("WARNING! Coincidental arcs !");
return 0;
}
double R0mR1Sqr = R0mR1*R0mR1;
if (USqrLen < R0mR1Sqr)
{
ret[0].set_valid( false );
ret[1].set_valid( false );
return 0;
}
double R0pR1 = R0 + R1;
double R0pR1Sqr = R0pR1*R0pR1;
if (USqrLen > R0pR1Sqr)
{
ret[0].set_valid( false );
ret[1].set_valid( false );
return 0;
}
if (USqrLen < R0pR1Sqr)
{
if (R0mR1Sqr < USqrLen)
{
double invUSqrLen = ((double)1)/USqrLen;
double s = (0.5)*((R0*R0-R1*R1)*invUSqrLen + 1.0);
vector_t tmp = center.get_data() + U*s;
// In theory, diskr is nonnegative. However, numerical round-off
// errors can make it slightly negative. Clamp it to zero.
double diskr = R0*R0*invUSqrLen - s*s;
if (diskr < PRX_ZERO_CHECK)
{
diskr = 0.0;
}
double t = std::sqrt(diskr);
vector_t V(U[1], -U[0]);
ret[0] = tmp - V*t;
ret[1] = tmp + V*t;
ret[0].set_valid( true );
ret[1].set_valid( true );
return 2;
}
else
{
//PRX_LOG_DEBUG ("|U| = |R0-R1|, circles are tangent. ");
ret[0] = center.get_data() + U*(R0/R0mR1);
ret[0].set_valid( true );
ret[1].set_valid( false );
return 1;
}
}
else
{
//PRX_LOG_DEBUG ("|U| = |R0+R1|, circles are tangent. ");
ret[0] = center.get_data() + U*(R0/R0pR1);
ret[0].set_valid( true );
ret[1].set_valid( false );
return 1;
}
}
int graph_proximity_t::find_k_close( abstract_node_t* state, proximity_node_t** close_nodes, double* distances, int k )
{
if( nr_nodes == 0 )
return 0;
added_nodes.clear();
if(k > MAX_KK)
{
PRX_WARN_S("Trying to return "<<k<<" points when the max is "<<MAX_KK);
k = MAX_KK;
}
if( k < nr_nodes )
{
for( int i=0; i<k; i++ )
{
bool exists = true;
int index;
while( exists == true )
{
index = rand() % nr_nodes;
exists = does_node_exist( nodes[index], close_nodes, i );
}
close_nodes[i] = nodes[index];
added_nodes[nodes[index]] = true;
distances[i] = nodes[index]->distance( state );
}
sort( close_nodes, distances, 0, k-1 );
int nr_samples = sampling_function();
double min_distance = distances[0];
int min_index = close_nodes[0]->get_index();
for( int i=0; i<nr_samples; i++ )
{
int index = rand() % nr_nodes;
if( does_node_exist( nodes[index], close_nodes, k ) == false )
{
double distance = nodes[index]->distance( state );
if( distance < min_distance )
{
min_distance = distance;
min_index = index;
}
}
}
if(min_distance!=distances[0])
{
added_nodes.erase(close_nodes[k-1]);
close_nodes[ k - 1 ] = nodes[min_index];
added_nodes[nodes[min_index]];
distances[ k - 1 ] = min_distance;
resort( close_nodes, distances, k-1 );
}
min_index = 0;
int last_index = close_nodes[k-1]->get_index();
int old_last_index;
do
{
old_last_index = last_index;
int nr_neighbors;
unsigned int* neighbors = nodes[ close_nodes[min_index]->get_index() ]->get_neighbors( &nr_neighbors );
for( int j=0; j<nr_neighbors; j++ )
{
if( does_node_exist( nodes[ neighbors[j] ], close_nodes, k ) == false )
{
double distance = nodes[ neighbors[j] ]->distance( state );
if( distance < distances[k-1] )
{
added_nodes.erase(close_nodes[k-1]);
close_nodes[k-1] = nodes[ neighbors[j] ];
added_nodes[nodes[neighbors[j]]];
distances[k-1] = distance;
resort( close_nodes, distances, k-1 );
last_index = close_nodes[k-1]->get_index();
}
}
}
min_index++;
}
while( min_index < k );
return k;
}
else
{
for( int i=0; i<nr_nodes; i++ )
{
close_nodes[i] = nodes[i];
distances[i] = nodes[i]->distance( state );
}
sort( close_nodes, distances, 0, nr_nodes-1 );
return nr_nodes;
}
}
