void planning_models::KinematicModel::RevoluteJointModel::getRandomValuesNearBy(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const Bounds &bounds,
const std::vector<double> &near, const double distance) const
{
if (continuous_)
{
values.push_back(rng.uniformReal(near[values.size()] - distance, near[values.size()] + distance));
enforceBounds(values, bounds);
}
else
values.push_back(rng.uniformReal(std::max(bounds[0].first, near[values.size()] - distance),
std::min(bounds[0].second, near[values.size()] + distance)));
}
void robot_model::PlanarJointModel::getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const Bounds &bounds) const
{
std::size_t s = values.size();
values.resize(s + 3);
if (bounds[0].second >= std::numeric_limits<double>::max() || bounds[0].first <= -std::numeric_limits<double>::max())
values[s] = 0.0;
else
values[s] = rng.uniformReal(bounds[0].first, bounds[0].second);
if (bounds[1].second >= std::numeric_limits<double>::max() || bounds[1].first <= -std::numeric_limits<double>::max())
values[s + 1] = 0.0;
else
values[s + 1] = rng.uniformReal(bounds[1].first, bounds[1].second);
values[s + 2] = rng.uniformReal(bounds[2].first, bounds[2].second);
}
void robot_model::PlanarJointModel::getVariableRandomValuesNearBy(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const Bounds &bounds,
const std::vector<double> &near, const double distance) const
{
std::size_t s = values.size();
values.resize(s + 3);
if (bounds[0].second >= std::numeric_limits<double>::max() || bounds[0].first <= -std::numeric_limits<double>::max())
values[s] = 0.0;
else
values[s] = rng.uniformReal(std::max(bounds[0].first, near[s] - distance),
std::min(bounds[0].second, near[s] + distance));
if (bounds[1].second >= std::numeric_limits<double>::max() || bounds[1].first <= -std::numeric_limits<double>::max())
values[s + 1] = 0.0;
else
values[s + 1] = rng.uniformReal(std::max(bounds[1].first, near[s + 1] - distance),
std::min(bounds[1].second, near[s + 1] + distance));
double da = angular_distance_weight_ * distance;
values[s + 2] = rng.uniformReal(near[s + 2] - da, near[s + 2] + da);
normalizeRotation(values);
}
void robot_model::FloatingJointModel::getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const Bounds &bounds) const
{
std::size_t s = values.size();
values.resize(s + 7);
if (bounds[0].second >= std::numeric_limits<double>::max() || bounds[0].first <= -std::numeric_limits<double>::max())
values[s] = 0.0;
else
values[s] = rng.uniformReal(bounds[0].first, bounds[0].second);
if (bounds[1].second >= std::numeric_limits<double>::max() || bounds[1].first <= -std::numeric_limits<double>::max())
values[s + 1] = 0.0;
else
values[s + 1] = rng.uniformReal(bounds[1].first, bounds[1].second);
if (bounds[2].second >= std::numeric_limits<double>::max() || bounds[2].first <= -std::numeric_limits<double>::max())
values[s + 2] = 0.0;
else
values[s + 2] = rng.uniformReal(bounds[2].first, bounds[2].second);
double q[4]; rng.quaternion(q);
values[s + 3] = q[0];
values[s + 4] = q[1];
values[s + 5] = q[2];
values[s + 6] = q[3];
}
bool playRandomReactionService(nao_animation_srvs::AnimationRandomPlay::Request &req,
nao_animation_srvs::AnimationRandomPlay::Response &res)
{
string body_status = req.animation_body_status;
string type = req.animation_motion_type;
string subtype = req.animation_motion_subtype;
if(body_status.compare(motion_state::body_state::STAND) == 0)
{
if(type.compare(motion_state::motion_type::REACTION) == 0)
{
if(subtype.compare(reaction_names::reaction_subtype::TOUCH_HEAD) == 0)
{
last_touch_head_index = touch_head_index;
touch_head_index = touch_head_random.uniformInteger(0, reaction_names::reaction_motion_names::reaction_motion_lists::NUM_STAND_TOUCH_HEAD -1);
if(reaction_names::reaction_motion_names::reaction_motion_lists::NUM_STAND_TOUCH_HEAD > 1)
{
while(last_touch_head_index == touch_head_index)
{
touch_head_index = touch_head_random.uniformInteger(0, reaction_names::reaction_motion_names::reaction_motion_lists::NUM_STAND_TOUCH_HEAD -1);
}
}
string motion = reaction_names::reaction_motion_names::reaction_motion_lists::stand_touch_head_list[touch_head_index];
motionProxy->angleInterpolationBezier(reactionMotions.getMotionNames(motion), reactionMotions.getMotionTimes(motion), reactionMotions.getMotionKeys(motion));
}
else if(subtype.compare(reaction_names::reaction_subtype::BUMPER_LEFT) == 0)
{
last_bumper_left_index = bumper_left_index;
bumper_left_index = bumper_left_random.uniformInteger(0, reaction_names::reaction_motion_names::reaction_motion_lists::NUM_STAND_BUMPER_LEFT -1);
if(reaction_names::reaction_motion_names::reaction_motion_lists::NUM_STAND_BUMPER_LEFT > 1)
{
while(last_bumper_left_index == bumper_left_index)
{
bumper_left_index = bumper_left_random.uniformInteger(0, reaction_names::reaction_motion_names::reaction_motion_lists::NUM_STAND_BUMPER_LEFT -1);
}
}
string motion = reaction_names::reaction_motion_names::reaction_motion_lists::stand_bumper_left_list[bumper_left_index];
motionProxy->angleInterpolationBezier(reactionMotions.getMotionNames(motion), reactionMotions.getMotionTimes(motion), reactionMotions.getMotionKeys(motion));
}
else if(subtype.compare(reaction_names::reaction_subtype::BUMPER_RIGHT) == 0)
{
last_bumper_right_index = bumper_right_index;
bumper_right_index = bumper_right_random.uniformInteger(0, reaction_names::reaction_motion_names::reaction_motion_lists::NUM_STAND_BUMPER_RIGHT -1);
if(reaction_names::reaction_motion_names::reaction_motion_lists::NUM_STAND_BUMPER_RIGHT > 1)
{
while(last_bumper_right_index == bumper_right_index)
{
bumper_right_index = bumper_right_random.uniformInteger(0, reaction_names::reaction_motion_names::reaction_motion_lists::NUM_STAND_BUMPER_RIGHT -1);
}
}
string motion = reaction_names::reaction_motion_names::reaction_motion_lists::stand_bumper_right_list[bumper_right_index];
motionProxy->angleInterpolationBezier(reactionMotions.getMotionNames(motion), reactionMotions.getMotionTimes(motion), reactionMotions.getMotionKeys(motion));
}
}
}
return true;
}
void robot_model::FloatingJointModel::getVariableRandomValuesNearBy(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const Bounds &bounds,
const std::vector<double> &near, const double distance) const
{
std::size_t s = values.size();
values.resize(s + 7);
if (bounds[0].second >= std::numeric_limits<double>::max() || bounds[0].first <= -std::numeric_limits<double>::max())
values[s] = 0.0;
else
values[s] = rng.uniformReal(std::max(bounds[0].first, near[s] - distance),
std::min(bounds[0].second, near[s] + distance));
if (bounds[1].second >= std::numeric_limits<double>::max() || bounds[1].first <= -std::numeric_limits<double>::max())
values[s + 1] = 0.0;
else
values[s + 1] = rng.uniformReal(std::max(bounds[1].first, near[s + 1] - distance),
std::min(bounds[1].second, near[s + 1] + distance));
if (bounds[2].second >= std::numeric_limits<double>::max() || bounds[2].first <= -std::numeric_limits<double>::max())
values[s + 2] = 0.0;
else
values[s + 2] = rng.uniformReal(std::max(bounds[2].first, near[s + 2] - distance),
std::min(bounds[2].second, near[s + 2] + distance));
double da = angular_distance_weight_ * distance;
if (da >= .25 * boost::math::constants::pi<double>())
{
double q[4]; rng.quaternion(q);
values[s + 3] = q[0];
values[s + 4] = q[1];
values[s + 5] = q[2];
values[s + 6] = q[3];
}
else
{
//taken from OMPL
// sample angle & axis
double ax = rng.gaussian01();
double ay = rng.gaussian01();
double az = rng.gaussian01();
double angle = 2.0 * pow(rng.uniform01(), 1.0/3.0) * da;
// convert to quaternion
double q[4];
double norm = sqrt(ax * ax + ay * ay + az * az);
if (norm < 1e-6)
{
q[0] = q[1] = q[2] = 0.0;
q[3] = 1.0;
}
else
{
double s = sin(angle / 2.0);
q[0] = s * ax / norm;
q[1] = s * ay / norm;
q[2] = s * az / norm;
q[3] = cos(angle / 2.0);
}
// multiply quaternions: near * q
values[s + 3] = near[s + 6]*q[0] + near[s + 3]*q[3] + near[s + 4]*q[2] - near[s + 5]*q[1];
values[s + 4] = near[s + 6]*q[1] + near[s + 4]*q[3] + near[s + 5]*q[0] - near[s + 3]*q[2];
values[s + 5] = near[s + 6]*q[2] + near[s + 5]*q[3] + near[s + 3]*q[1] - near[s + 4]*q[0];
values[s + 6] = near[s + 6]*q[3] - near[s + 3]*q[0] - near[s + 4]*q[1] - near[s + 5]*q[2];
}
}
void planning_models::KinematicModel::RevoluteJointModel::getRandomValues(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const Bounds &bounds) const
{
values.push_back(rng.uniformReal(bounds[0].first, bounds[0].second));
}