void MotionPlanningFrame::attachObjectToState(robot_state::RobotState& state,
                                               const std::string& object_id,
                                               const std::string& link_id,
                                               const geometry_msgs::Pose& pose_object_link)
 {
     state.clearAttachedBodies();
     if (object_id.empty() || link_id.empty())
         return;
     Eigen::Affine3d T_object_link;
     tf::poseMsgToEigen(pose_object_link, T_object_link);
     attachObjectToState(state, object_id, link_id, T_object_link);
 }
    void MotionPlanningFrame::attachObjectToState(robot_state::RobotState& state,
                                                  const std::string& object_id,
                                                  const std::string& link_id,
                                                  const Eigen::Affine3d& T_object_link)
    {
        state.clearAttachedBodies();
        if (object_id.empty())
            return;

        // Compute the object key.
        KeyPoseMap world_state = computeWorldKeyPoseMap();
        std::string object_key = computeNearestFrameKey(object_id, link_id, state, world_state);

        // Extract the object from the world.
        collision_detection::CollisionWorld::ObjectConstPtr object;
        std::vector<shapes::ShapeConstPtr> shapes;
        EigenSTL::vector_Affine3d poses;
        {
            planning_scene_monitor::LockedPlanningSceneRO ps = planning_display_->getPlanningSceneRO();
            object = ps->getWorld()->getObject(object_key);
        }
        if (!object)
        {
            if (!object_id.empty())
                ROS_WARN("Failed to attach %s to %s. Object does not exist.",
                         object_id.c_str(), link_id.c_str());
            return;
        }

        // Compute poses relative to link.
        shapes = object->shapes_;
        poses = object->shape_poses_;
        for (int i = 0; i < poses.size(); i++)
            poses[i] = T_object_link * poses[0].inverse() * poses[i];
        // Attach collision object to robot.
        moveit_msgs::AttachedCollisionObject aco; // For dummy arguments.
        state.attachBody(object_id, shapes, poses, aco.touch_links, link_id, aco.detach_posture);
        // Update state.
        state.update();
    }
    void MotionPlanningFrame::snapStateToFrame(robot_state::RobotState& state,
                                               const Eigen::Affine3d& T_frame_world,
                                               const std::string& link,
                                               const Eigen::Affine3d& T_frame_link,
                                               const std::string& group)
    {
        ROS_DEBUG_FUNCTION;

        // std::cout << "T_frame_world:\n" << T_frame_world.matrix() << std::endl;
        // std::cout << "T_frame_link:\n" << T_frame_link.matrix() << std::endl;

        // Back out the desired link transform in global coordinates.
        Eigen::Affine3d T_link_world = T_frame_world * T_frame_link.inverse();
        // Snap to the frame using IK.
        const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(group);

        // std::cout << "group: " << group << std::endl;

        state.update();
        trajectory_msgs::JointTrajectoryPoint point;
        state.copyJointGroupPositions(jmg, point.positions);
        ROS_DEBUG_STREAM("pre-ik positions:\n" << point);

        geometry_msgs::Pose pose;
        tf::poseEigenToMsg(T_link_world, pose);
        state.setFromIK(jmg, pose, link);
        state.update();

        state.copyJointGroupPositions(jmg, point.positions);
        ROS_DEBUG_STREAM("post-ik positions:\n" << point);

        ROS_DEBUG_FUNCTION;
    }
 void MotionPlanningFrame::computeLinearInterpPlan(const robot_state::RobotState& start,
                                                   apc_msgs::PrimitiveAction& goal)
 {
     std::string group = goal.group_name;
     const moveit::core::JointModelGroup* joint_group = start.getJointModelGroup(group);
     trajectory_msgs::JointTrajectoryPoint s, c, e;
     const std::vector<std::string>& joint_names = goal.joint_trajectory.joint_names;
     s = goal.joint_trajectory.points.front();
     for (int i = 0; i < joint_names.size(); i++)
         s.positions[i] = (start.getVariablePosition(joint_names[i]));
     e = goal.joint_trajectory.points.back();
     goal.joint_trajectory.points.clear();
     trajectory_msgs::JointTrajectory& T = goal.joint_trajectory;
     const int n = 15;
     c = s;
     for (int i = 0; i <= n; i++)
     {
         for (int j=0; j < c.positions.size(); j++)
             c.positions[j] = s.positions[j] + i * (e.positions[j] - s.positions[j]) / n;
         T.points.push_back(c);
     }
 }
void ompl_interface::ModelBasedStateSpace::copyJointToOMPLState(ompl::base::State* state,
                                                                const robot_state::RobotState& robot_state,
                                                                const moveit::core::JointModel* joint_model,
                                                                int ompl_state_joint_index) const
{
  // Copy one joint (multiple variables possibly)
  memcpy(getValueAddressAtIndex(state, ompl_state_joint_index),
         robot_state.getVariablePositions() + joint_model->getFirstVariableIndex() * sizeof(double),
         joint_model->getVariableCount() * sizeof(double));

  // clear any cached info (such as validity known or not)
  state->as<StateType>()->clearKnownInformation();
}
예제 #6
0
// This is intended to be called as a ModifyStateFunction to modify the state
// maintained by a LockedRobotState in place.
bool robot_interaction::KinematicOptions::setStateFromIK(
      robot_state::RobotState& state,
      const std::string& group,
      const std::string& tip,
      const geometry_msgs::Pose& pose) const
{
  const robot_model::JointModelGroup *jmg = state.getJointModelGroup(group);
  if (!jmg)
  {
    ROS_ERROR("No getJointModelGroup('%s') found",group.c_str());
    return false;
  }
  bool result = state.setFromIK(jmg,
                            pose,
                            tip,
                            max_attempts_,
                            timeout_seconds_,
                            state_validity_callback_,
                            options_);
  state.update();
  return result;
}
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::JointConstraint::decide(const robot_state::RobotState &state, bool verbose) const
{
  if (!joint_model_)
    return ConstraintEvaluationResult(true, 0.0);

  const robot_state::JointState *joint = state.getJointState(joint_model_->getName());

  if (!joint)
  {
    logWarn("No joint in state with name '%s'", joint_model_->getName().c_str());
    return ConstraintEvaluationResult(true, 0.0);
  }

  double current_joint_position = joint->getVariableValues()[0];
  if (!local_variable_name_.empty())
  {
    const std::map<std::string, unsigned int> &index_map = joint->getVariableIndexMap();
    std::map<std::string, unsigned int>::const_iterator it = index_map.find(joint_variable_name_);
    if (it == index_map.end())
    {
      logWarn("Local name '%s' is not known to joint state with name '%s'", local_variable_name_.c_str(), joint_model_->getName().c_str());
      return ConstraintEvaluationResult(true, 0.0);
    }
    else
      current_joint_position = joint->getVariableValues()[it->second];
  }

  double dif = 0.0;

  // compute signed shortest distance for continuous joints
  if (joint_is_continuous_)
  {
    dif = normalizeAngle(current_joint_position) - joint_position_;

    if (dif > boost::math::constants::pi<double>())
      dif = 2.0*boost::math::constants::pi<double>() - dif;
    else
      if (dif < -boost::math::constants::pi<double>())
        dif += 2.0*boost::math::constants::pi<double>(); // we include a sign change to have dif > 0
  }
  else
    dif = current_joint_position - joint_position_;

  // check bounds
  bool result = dif <= (joint_tolerance_above_+2*std::numeric_limits<double>::epsilon()) && dif >= (-joint_tolerance_below_-2*std::numeric_limits<double>::epsilon());
  if (verbose)
    logInform("Constraint %s:: Joint name: '%s', actual value: %f, desired value: %f, tolerance_above: %f, tolerance_below: %f",
              result ? "satisfied" : "violated", joint_variable_name_.c_str(),
              current_joint_position, joint_position_, joint_tolerance_above_, joint_tolerance_below_);
  return ConstraintEvaluationResult(result, constraint_weight_ * fabs(dif));
}
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::OrientationConstraint::decide(const robot_state::RobotState &state, bool verbose) const
{
  if (!link_model_)
    return ConstraintEvaluationResult(true, 0.0);

  const robot_state::LinkState *link_state = state.getLinkState(link_model_->getName());

  if (!link_state)
  {
    logWarn("No link in state with name '%s'", link_model_->getName().c_str());
    return ConstraintEvaluationResult(false, 0.0);
  }

  Eigen::Vector3d xyz;
  if (mobile_frame_)
  {
    Eigen::Matrix3d tmp;
    tf_->transformRotationMatrix(state, desired_rotation_frame_id_, desired_rotation_matrix_, tmp);
    Eigen::Affine3d diff(tmp.inverse() * link_state->getGlobalLinkTransform().rotation());
    xyz = diff.rotation().eulerAngles(0, 1, 2);
    // 0,1,2 corresponds to ZXZ, the convention used in sampling constraints
  }
  else
  {
    Eigen::Affine3d diff(desired_rotation_matrix_inv_ * link_state->getGlobalLinkTransform().rotation());
    xyz = diff.rotation().eulerAngles(0, 1, 2); // 0,1,2 corresponds to ZXZ, the convention used in sampling constraints
  }

  xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::pi<double>() - fabs(xyz(0)));
  xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::pi<double>() - fabs(xyz(1)));
  xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::pi<double>() - fabs(xyz(2)));
  bool result = xyz(2) < absolute_z_axis_tolerance_+std::numeric_limits<double>::epsilon()
    && xyz(1) < absolute_y_axis_tolerance_+std::numeric_limits<double>::epsilon()
    && xyz(0) < absolute_x_axis_tolerance_+std::numeric_limits<double>::epsilon();

  if (verbose)
  {
    Eigen::Quaterniond q_act(link_state->getGlobalLinkTransform().rotation());
    Eigen::Quaterniond q_des(desired_rotation_matrix_);
    logInform("Orientation constraint %s for link '%s'. Quaternion desired: %f %f %f %f, quaternion actual: %f %f %f %f, error: x=%f, y=%f, z=%f, tolerance: x=%f, y=%f, z=%f",
             result ? "satisfied" : "violated", link_model_->getName().c_str(),
             q_des.x(), q_des.y(), q_des.z(), q_des.w(),
             q_act.x(), q_act.y(), q_act.z(), q_act.w(), xyz(0), xyz(1), xyz(2),
             absolute_x_axis_tolerance_, absolute_y_axis_tolerance_, absolute_z_axis_tolerance_);
  }

  return ConstraintEvaluationResult(result, constraint_weight_ * (xyz(0) + xyz(1) + xyz(2)));
}
예제 #9
0
bool constraint_samplers::UnionConstraintSampler::sample(robot_state::RobotState &state, const robot_state::RobotState &reference_state, unsigned int max_attempts)
{
  state = reference_state;
  state.setToRandomPositions(jmg_);
  
  if (samplers_.size() >= 1)
  {
    if (!samplers_[0]->sample(state, reference_state, max_attempts))
      return false;
  }
  
  for (std::size_t i = 1 ; i < samplers_.size() ; ++i)
    if (!samplers_[i]->sample(state, state, max_attempts))
      return false;
  
  return true;
}
예제 #10
0
bool KinematicsMetrics::getManipulabilityEllipsoid(const robot_state::RobotState &state,
                                                   const robot_model::JointModelGroup *joint_model_group,
                                                   Eigen::MatrixXcd &eigen_values,
                                                   Eigen::MatrixXcd &eigen_vectors) const
{
  // state.getJacobian() only works for chain groups.
  if(!joint_model_group->isChain())
  {
    return false;
  }

  Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
  Eigen::MatrixXd matrix = jacobian*jacobian.transpose();
  Eigen::EigenSolver<Eigen::MatrixXd> eigensolver(matrix.block(0, 0, 3, 3));
  eigen_values = eigensolver.eigenvalues();
  eigen_vectors = eigensolver.eigenvectors();
  return true;
}
예제 #11
0
파일: utils.cpp 프로젝트: ksenglee/ros
moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const robot_state::RobotState &state, const robot_model::JointModelGroup *jmg,
                                                                         double tolerance_below, double tolerance_above)
{
  moveit_msgs::Constraints goal;
  std::vector<double> vals;
  state.copyJointGroupPositions(jmg, vals);
  goal.joint_constraints.resize(vals.size());
  for (std::size_t i = 0 ; i < vals.size() ; ++i)
  {
    goal.joint_constraints[i].joint_name = jmg->getVariableNames()[i];
    goal.joint_constraints[i].position = vals[i];
    goal.joint_constraints[i].tolerance_above = tolerance_below;
    goal.joint_constraints[i].tolerance_below = tolerance_above;
    goal.joint_constraints[i].weight = 1.0;
  }

  return goal;
}
예제 #12
0
double KinematicsMetrics::getJointLimitsPenalty(const robot_state::RobotState &state, const robot_model::JointModelGroup *joint_model_group) const
{
  if (fabs(penalty_multiplier_) <= boost::math::tools::epsilon<double>())
     return 1.0;
  double joint_limits_multiplier(1.0);
  const std::vector<const robot_model::JointModel*> &joint_model_vector = joint_model_group->getJointModels();
  for (std::size_t i = 0; i < joint_model_vector.size(); ++i)
  {
    if (joint_model_vector[i]->getType() == robot_model::JointModel::REVOLUTE)
    {
      const robot_model::RevoluteJointModel* revolute_model = static_cast<const robot_model::RevoluteJointModel*>(joint_model_vector[i]);
      if (revolute_model->isContinuous())
        continue;
    }
    if (joint_model_vector[i]->getType() == robot_model::JointModel::PLANAR)
    {
      const robot_model::JointModel::Bounds &bounds = joint_model_vector[i]->getVariableBounds();
      if (bounds[0].min_position_ == -std::numeric_limits<double>::max() || bounds[0].max_position_ == std::numeric_limits<double>::max() ||
          bounds[1].min_position_ == -std::numeric_limits<double>::max() || bounds[1].max_position_ == std::numeric_limits<double>::max() ||
          bounds[2].min_position_ == -boost::math::constants::pi<double>() || bounds[2].max_position_ == boost::math::constants::pi<double>())
        continue;
    }
    if (joint_model_vector[i]->getType() == robot_model::JointModel::FLOATING)
    {
      //Joint limits are not well-defined for floating joints
      continue;
    }
    const double *joint_values = state.getJointPositions(joint_model_vector[i]);
    const robot_model::JointModel::Bounds &bounds = joint_model_vector[i]->getVariableBounds();
    std::vector<double> lower_bounds, upper_bounds;
    for (std::size_t j = 0; j < bounds.size(); ++j)
    {
      lower_bounds.push_back(bounds[j].min_position_);
      upper_bounds.push_back(bounds[j].max_position_);
    }
    double lower_bound_distance = joint_model_vector[i]->distance(joint_values, &lower_bounds[0]);
    double upper_bound_distance = joint_model_vector[i]->distance(joint_values, &upper_bounds[0]);
    double range = lower_bound_distance + upper_bound_distance;
    if (range <= boost::math::tools::epsilon<double>())
      continue;
    joint_limits_multiplier *= (lower_bound_distance * upper_bound_distance/(range*range));
  }
  return (1.0 - exp(-penalty_multiplier_*joint_limits_multiplier));
}
void constraint_samplers::visualizeDistribution(const ConstraintSamplerPtr& sampler,
                                                const robot_state::RobotState& reference_state,
                                                const std::string& link_name, unsigned int sample_count,
                                                visualization_msgs::MarkerArray& markers)
{
  if (!sampler)
  {
    ROS_ERROR_NAMED("constraint_samplers", "No sampler specified for visualizing distribution of samples");
    return;
  }
  const robot_state::LinkModel* lm = reference_state.getLinkModel(link_name);
  if (!lm)
    return;
  robot_state::RobotState ks(reference_state);
  std_msgs::ColorRGBA color;
  color.r = 1.0f;
  color.g = 0.0f;
  color.b = 0.0f;
  color.a = 1.0f;
  for (unsigned int i = 0; i < sample_count; ++i)
  {
    if (!sampler->sample(ks))
      continue;
    const Eigen::Vector3d& pos = ks.getGlobalLinkTransform(lm).translation();
    visualization_msgs::Marker mk;
    mk.header.stamp = ros::Time::now();
    mk.header.frame_id = sampler->getJointModelGroup()->getParentModel().getModelFrame();
    mk.ns = "constraint_samples";
    mk.id = i;
    mk.type = visualization_msgs::Marker::SPHERE;
    mk.action = visualization_msgs::Marker::ADD;
    mk.pose.position.x = pos.x();
    mk.pose.position.y = pos.y();
    mk.pose.position.z = pos.z();
    mk.pose.orientation.w = 1.0;
    mk.scale.x = mk.scale.y = mk.scale.z = 0.035;
    mk.color = color;
    mk.lifetime = ros::Duration(30.0);
    markers.markers.push_back(mk);
  }
}
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::PositionConstraint::decide(const robot_state::RobotState &state, bool verbose) const
{
  if (!link_model_ || constraint_region_.empty())
    return ConstraintEvaluationResult(true, 0.0);

  const robot_state::LinkState *link_state = state.getLinkState(link_model_->getName());

  if (!link_state)
  {
    logWarn("No link in state with name '%s'", link_model_->getName().c_str());
    return ConstraintEvaluationResult(false, 0.0);
  }

  Eigen::Vector3d pt = link_state->getGlobalLinkTransform() * offset_;
  if (mobile_frame_)
  {
    Eigen::Affine3d tmp;
    for (std::size_t i = 0 ; i < constraint_region_.size() ; ++i)
    {
      tf_->transformPose(state, constraint_frame_id_, constraint_region_pose_[i], tmp);
      bool result = constraint_region_[i]->cloneAt(tmp)->containsPoint(pt, verbose);
      if (result || (i + 1 == constraint_region_pose_.size()))
        return finishPositionConstraintDecision(pt, tmp.translation(), link_model_->getName(), constraint_weight_, result, verbose);
      else
        finishPositionConstraintDecision(pt, tmp.translation(), link_model_->getName(), constraint_weight_, result, verbose);
    }
  }
  else
  {
    for (std::size_t i = 0 ; i < constraint_region_.size() ; ++i)
    {
      bool result = constraint_region_[i]->containsPoint(pt, true);
      if (result || (i + 1 == constraint_region_.size()))
        return finishPositionConstraintDecision(pt, constraint_region_[i]->getPose().translation(), link_model_->getName(), constraint_weight_, result, verbose);
      else
        finishPositionConstraintDecision(pt, constraint_region_[i]->getPose().translation(), link_model_->getName(), constraint_weight_, result, verbose);
    }
  }
  return ConstraintEvaluationResult(false, 0.0);
}
예제 #15
0
void displayInitialWaypoints(robot_state::RobotState& state,
                             ros::NodeHandle& node_handle,
                             robot_model::RobotModelPtr& robot_model,
                             const std::vector<std::string>& hierarchy,
                             const std::vector<Eigen::VectorXd>& waypoints)
{
    static ros::Publisher vis_marker_array_publisher = node_handle.advertise<visualization_msgs::MarkerArray>("/move_itomp/visualization_marker_array", 10);

    visualization_msgs::MarkerArray ma;
    std::vector<std::string> link_names = robot_model->getLinkModelNames();
    std_msgs::ColorRGBA color;
    color.a = 0.5;
    color.r = 1.0;
    color.g = 1.0;
    color.b = 0.0;

    //ros::Duration dur(3600.0);
    ros::Duration dur(0.25);

    for (unsigned int point = 0; point < waypoints.size(); ++point)
    {
        ma.markers.clear();

        setRobotStateFrom(state, hierarchy, waypoints, point);


        double time = 0.05;
        ros::WallDuration timer(time);
        timer.sleep();


        std::string ns = "init_" + boost::lexical_cast<std::string>(point);
        state.getRobotMarkers(ma, link_names, color, ns, dur);
        vis_marker_array_publisher.publish(ma);
    }
}
예제 #16
0
bool userCallback(const robot_state::RobotState &kinematic_state, bool verbose)
{
  // get the joint value for the right shoulder pan of the PR2 robot
    const std::vector<double>& joint_state_values = kinematic_state.getJointState("shoulder_pan_joint")->getVariableValues();
    return (joint_state_values.front() > 0.0);
}
예제 #17
0
void planning_scene_monitor::CurrentStateMonitor::setToCurrentState(robot_state::RobotState &upd) const
{
  boost::mutex::scoped_lock slock(state_update_lock_);
  const double *pos = robot_state_.getVariablePositions();
  upd.setVariablePositions(pos);
}
예제 #18
0
void ompl_interface::ModelBasedStateSpace::copyToOMPLState(ompl::base::State *state, const robot_state::RobotState &rstate) const
{
  rstate.copyJointGroupPositions(spec_.joint_model_group_, state->as<StateType>()->values);
  // clear any cached info (such as validity known or not)
  state->as<StateType>()->clearKnownInformation();
}
예제 #19
0
void ompl_interface::ModelBasedStateSpace::copyToRobotState(robot_state::RobotState& rstate, const ompl::base::State *state) const
{
  rstate.setJointGroupPositions(spec_.joint_model_group_, state->as<StateType>()->values);
  rstate.update();
}
// BEGIN_SUB_TUTORIAL userCallback
//
// User defined constraints can also be specified to the PlanningScene
// class. This is done by specifying a callback using the
// setStateFeasibilityPredicate function. Here's a simple example of a
// user-defined callback that checks whether the "r_shoulder_pan" of
// the PR2 robot is at a positive or negative angle:
bool userCallback(const robot_state::RobotState &kinematic_state, bool verbose)
{
  const double* joint_values = kinematic_state.getJointPositions("shoulder_pan_joint");
  return (joint_values[0] > 0.0);
}
void MotionPlanningFrame::updateQueryStateHelper(robot_state::RobotState &state, const std::string &v)
{
  if (v == "<random>")
  {
    configureWorkspace();
    if (const robot_model::JointModelGroup *jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup()))
      state.setToRandomPositions(jmg);
  }
  else
    if (v == "<random valid>")
    {
      configureWorkspace();

      if (const robot_model::JointModelGroup *jmg =
        state.getJointModelGroup(planning_display_->getCurrentPlanningGroup()))
      {
        // Loop until a collision free state is found
        static const int MAX_ATTEMPTS = 100;
        int attempt_count = 0; // prevent loop for going forever
        while (attempt_count < MAX_ATTEMPTS)
        {
          // Generate random state
          state.setToRandomPositions(jmg);

          state.update(); // prevent dirty transforms

          // Test for collision
          if (planning_display_->getPlanningSceneRO()->isStateValid(state, "", false))
            break;

          attempt_count ++;
        }
        // Explain if no valid rand state found
        if (attempt_count >= MAX_ATTEMPTS)
          ROS_WARN("Unable to find a random collision free configuration after %d attempts", MAX_ATTEMPTS);
      }
      else
      {
        ROS_WARN_STREAM("Unable to get joint model group " << planning_display_->getCurrentPlanningGroup());
      }
    }
    else
      if (v == "<current>")
      {
        const planning_scene_monitor::LockedPlanningSceneRO &ps = planning_display_->getPlanningSceneRO();
        if (ps)
          state = ps->getCurrentState();
      }
      else
        if (v == "<same as goal>")
        {
          state = *planning_display_->getQueryGoalState();
        }
        else
          if (v == "<same as start>")
          {
            state = *planning_display_->getQueryStartState();
          }
          else
          {
            // maybe it is a named state
            if (const robot_model::JointModelGroup *jmg = state.getJointModelGroup(planning_display_->getCurrentPlanningGroup()))
              state.setToDefaultValues(jmg, v);
          }
}