double quaternion_t::distance( const quaternion_t& other ) const
{
PRX_ASSERT_MSG(is_valid(),
"Invalid quaternion: %f %f %f %f", q[0], q[1], q[2], q[3]);
PRX_ASSERT_MSG(other.is_valid(),
"Invalid quaternion: %f %f %f %f",
other.q[0], other.q[1], other.q[2], other.q[3]);
const vector_t v1(q[0],q[1],q[2],q[3]);
const vector_t v2(other.q[0], other.q[1], other.q[2], other.q[3]);
const double lambda = v1.dot_product(v2);
return 1 - fabs(lambda);
}
void minimal_avoidance_controller_t::init(const parameter_reader_t * reader, const parameter_reader_t* template_reader)
{
simple_controller_t::init(reader,template_reader);
obstacle_distance = parameters::get_attribute_as<double>("obstacle_distance", reader, template_reader);
plant_distance = parameters::get_attribute_as<double>("plant_distance", reader, template_reader);
for(unsigned i=0; i < this->sensing_info.size() && prox_info == NULL; ++i)
{
prox_info = dynamic_cast<twoD_proximity_sensing_info_t*>(sensing_info[i]);
}
PRX_ASSERT_MSG(prox_info != NULL, "Minimal avoidance controller needs a proximity sensing info");
contingency_plan.link_control_space(output_control_space);
subsystems.begin()->second->append_contingency(contingency_plan, 0.0);
input_control_space = output_control_space;
}
