C++ robot_state::RobotState类代码示例

 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::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();
}

// 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));
}

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;
}

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;
}

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;
}

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);
  }
}

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);
    }
}

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);
}

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);
}

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();
}