3.5 关节空间规划¶

MoveGroup 类¶

在 MoveIt!，最简单的是通过使用 MoveGroup 类。它为用户的大多数操作提供了简单的函数接口，特别是设置关节目标和位姿目标、创建运动规划、移动机器人、增加物体到环境中、增加/减少机器人本身的物体。这个接口允许你通过ROS 话题、服务和action与MoveGroup节点通信。

执行关节空间的运动规划¶

1、启动 RViz 仿真环境：

roslaunch gauss_moveit_config demo.launch

2、在新的终端执行：

rosrun tony_moveit_tutorials move_group_interface_joint

我们可以设置机械臂各个关节的角度，并执行运动规划。在RViz仿真环境中可以看到机械臂的 1 轴转过了90度。

plan_in_joint_space

代码解析¶

1、初始化 ros node：

ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();

2、设置move group：

moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

3、获取关节状态：

// Raw pointers are frequently used to refer to the planning group for improved performance.
const robot_state::JointModelGroup* joint_model_group =
    move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

// Planning to a joint-space goal
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//
// Let's set a joint space goal and move towards it.  This will replace the
// pose target we set above.
//
// To start, we'll create an pointer that references the current robot's state.
// RobotState is the object that contains all the current position/velocity/acceleration data.
moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
//
// Next get the current set of joint values for the group.
std::vector<double> joint_group_positions;
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);

4、使用 MoveIt! 做运动规划：

// Now, let's modify one of the joints, plan to the new joint space goal and visualize the plan.
joint_group_positions[0] = -3.1415926/2;  // radians
move_group.setJointValueTarget(joint_group_positions);

moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");    

5、执行运动规划：

move_group.move();