Ejemplo n.º 1
0
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];
  }
}
Ejemplo n.º 7
0
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));
}