C++ RobotState::getJointModelGroup方法代码示例

    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 setWalkingStates(robot_state::RobotState& start_state,
        robot_state::RobotState& goal_state, Eigen::VectorXd& start_transf,
        Eigen::VectorXd& goal_transf, const std::vector<std::string>& hierarchy)
{
	std::map<std::string, double> values;
	double jointValue = 0.0;

	const robot_state::JointModelGroup* joint_model_group =
			start_state.getJointModelGroup("whole_body");

	joint_model_group->getVariableDefaultPositions("standup", values);
	start_state.setVariablePositions(values);
    int id = 0;
    for(std::vector<std::string>::const_iterator cit = hierarchy.begin();
        cit != hierarchy.end(); ++cit, ++id)
    {
        jointValue = start_transf(id);
        start_state.setJointPositions(*cit, &jointValue);
    }

    goal_state = start_state;
    joint_model_group->getVariableDefaultPositions("standup", values);
    goal_state.setVariablePositions(values);

    id = 0;
    for(std::vector<std::string>::const_iterator cit = hierarchy.begin();
        cit != hierarchy.end(); ++cit, ++id)
    {
        jointValue = goal_transf(id);
        goal_state.setJointPositions(*cit, &jointValue);
    }
}

void doPlan(const std::string& group_name,
		planning_interface::MotionPlanRequest& req,
		planning_interface::MotionPlanResponse& res,
		robot_state::RobotState& start_state,
		robot_state::RobotState& goal_state,
		planning_scene::PlanningScenePtr& planning_scene,
		planning_interface::PlannerManagerPtr& planner_instance)
{
	const robot_state::JointModelGroup* joint_model_group =
			goal_state.getJointModelGroup("whole_body");

	// Copy from start_state to req.start_state
	unsigned int num_joints = start_state.getVariableCount();
	req.start_state.joint_state.name = start_state.getVariableNames();
	req.start_state.joint_state.position.resize(num_joints);
	req.start_state.joint_state.velocity.resize(num_joints);
	req.start_state.joint_state.effort.resize(num_joints);
	memcpy(&req.start_state.joint_state.position[0],
			start_state.getVariablePositions(), sizeof(double) * num_joints);
	if (start_state.hasVelocities())
		memcpy(&req.start_state.joint_state.velocity[0],
				start_state.getVariableVelocities(),
				sizeof(double) * num_joints);
	else
		memset(&req.start_state.joint_state.velocity[0], 0,
				sizeof(double) * num_joints);
	if (start_state.hasAccelerations())
		memcpy(&req.start_state.joint_state.effort[0],
				start_state.getVariableAccelerations(),
				sizeof(double) * num_joints);
	else
		memset(&req.start_state.joint_state.effort[0], 0,
				sizeof(double) * num_joints);

	req.group_name = group_name;
	moveit_msgs::Constraints joint_goal =
			kinematic_constraints::constructGoalConstraints(goal_state,
					joint_model_group);
	req.goal_constraints.push_back(joint_goal);

	// We now construct a planning context that encapsulate the scene,
	// the request and the response. We call the planner using this
	// planning context
	planning_interface::PlanningContextPtr context =
			planner_instance->getPlanningContext(planning_scene, req,
					res.error_code_);
	context->solve(res);
	if (res.error_code_.val != res.error_code_.SUCCESS)
	{
		ROS_ERROR("Could not compute plan successfully");
		exit(0);
	}
}

    void MotionPlanningFrame::appendStateToAction(apc_msgs::PrimitiveAction& action,
                                                  const robot_state::RobotState& state)
    {
        APC_ASSERT(!action.group_id.empty(), "Failed to find group in action");

        const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(action.group_id);
        APC_ASSERT(jmg, "Failed to get joint model group %s", action.group_id.c_str());

        // Append joint angles of state to the joint trajectory.
        std::vector<std::string> joint_names = jmg->getVariableNames(); //jmg->getActiveJointModelNames();
        std::vector<std::string> variable_names = jmg->getVariableNames();

        // Set joint variable names if they are missing.
        if (action.joint_trajectory.joint_names.size() == 0) {
            action.joint_trajectory.joint_names = joint_names;
        } else if (action.joint_trajectory.joint_names.size() != joint_names.size()) {
            action.joint_trajectory.joint_names = joint_names;
        }
        action.joint_trajectory.joint_names = joint_names; // HACK

        // Append joint angles of state to the joint trajectory.
        Eigen::VectorXd variable_values;
        state.copyJointGroupPositions(jmg, variable_values);
        trajectory_msgs::JointTrajectoryPoint point;
        int appended = 0;
        for (int i = 0; i < variable_values.size(); i++) {
            if (std::find(joint_names.begin(), joint_names.end(), variable_names[i]) != joint_names.end()) {
                point.positions.push_back(variable_values[i]);
                appended++;
            }
        }
        action.joint_trajectory.points.push_back(point);

        // Assert that the number of joint names in the input action
        // match the number positions in the point.
        APC_ASSERT(appended == action.joint_trajectory.joint_names.size(),
                   "Mismatch in number of positions when appending state to action");

        // Assert that the joint names match the active joints of the
        // group.
        const std::vector<std::string>& action_joint_names = action.joint_trajectory.joint_names;
        for (int i = 0; i < action_joint_names.size(); i++)
            APC_ASSERT(std::find(joint_names.begin(), joint_names.end(), action_joint_names[i]) != joint_names.end(),
                       "Failed to find joint %s in group %s", action_joint_names[i].c_str(), action.group_id.c_str());
        for (int i = 0; i < joint_names.size(); i++)
            APC_ASSERT(std::find(action_joint_names.begin(), action_joint_names.end(), joint_names[i]) != action_joint_names.end(),
                       "Failed to find joint %s in group %s", joint_names[i].c_str(), action.group_id.c_str());
    }

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

void setRobotStateFrom(robot_state::RobotState& state,
                       const std::vector<std::string>& hierarchy,
                       const std::vector<Eigen::VectorXd>& waypoints,
                       int index)
{
    std::map<std::string, double> values;
    double jointValue = 0.0;
    const robot_state::JointModelGroup* joint_model_group = state.getJointModelGroup("whole_body");

    joint_model_group->getVariableDefaultPositions("standup", values);
    state.setVariablePositions(values);

    int id = 0;
    for(std::vector<std::string>::const_iterator cit = hierarchy.begin(); cit != hierarchy.end(); ++cit, ++id)
    {
        jointValue = waypoints[index](id);
        state.setJointPositions(*cit, &jointValue);
    }
}

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

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