abuo学习2：修改关节角度

完整代码

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>

#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>

#include <moveit_visual_tools/moveit_visual_tools.h>
#include <tf/LinearMath/Quaternion.h>

int main(int argc, char **argv)
{
ros::init(argc, argv, "MoveGroupInterface_To_Noetic");
ros::NodeHandle node_handle;

// Start a thread
ros::AsyncSpinner spinner(1);
spinner.start();

// Define the planning group name
static const std::string PLANNING_GROUP = "manipulator_i5";

// Create a planning group interface object and set up a planning group
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
move_group.setPoseReferenceFrame("base_link");

// Create a planning scene interface object
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

// Create a robot model information object
const robot_state::JointModelGroup* joint_model_group = move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

// Create an object of the visualization class
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
visual_tools.deleteAllMarkers();

// Load remote control tool
visual_tools.loadRemoteControl();

// Create text
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.2;
visual_tools.publishText(text_pose, "AUBO Demo", rvt::RED, rvt::XLARGE);
// Text visualization takes effect
visual_tools.trigger();

// Get the coordinate system of the basic information
ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str());

// Get the end of the basic information
ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());

// Visual terminal prompt (blocking)
// visual_tools.prompt("Press 'next'1 in the RvizVisualToolsGui window to start the demo");

//*************Home Position

std::vector<double> home_position;
home_position.push_back(-0.001255);
home_position.push_back(-0.148822);
home_position.push_back(-1.406503);
home_position.push_back(0.311441);
home_position.push_back(-1.571295);
home_position.push_back(-0.002450);
move_group.setJointValueTarget(home_position);
move_group.move();

//*******The second example, the joint 1 is rotated 90 degrees based on the home position.

moveit::core::RobotStatePtr current_state = move_group.getCurrentState();

// Get the joint value and model information of the current group
std::vector<double> joint_group_positions;
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);

// Planning result container and success flag
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = false;

// Modify the value of joint 1
joint_group_positions[0] = -1.57; // radians
move_group.setJointValueTarget(joint_group_positions);

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

// Visual display in RVIZ
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "AUBO Joint Space Goal Example2", rvt::RED, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();

// Perform planning actions
move_group.execute(my_plan);

// Move to the home point position
joint_group_positions[0] = 0; // radians
move_group.setJointValueTarget(joint_group_positions);
move_group.move();

ros::shutdown();
return 0;

}

代码分析

1. 头文件包含

#include <moveit/move_group_interface/move_group_interface.h> // MoveIt核心接口
#include <moveit/planning_scene_interface/planning_scene_interface.h> // 规划场景接口
#include <moveit_msgs/DisplayRobotState.h> // 机器人状态显示
#include <moveit_msgs/DisplayTrajectory.h> // 轨迹显示
#include <moveit_msgs/AttachedCollisionObject.h> // 附着碰撞对象
#include <moveit_msgs/CollisionObject.h> // 碰撞对象
#include <moveit_visual_tools/moveit_visual_tools.h> // 可视化工具
#include <tf/LinearMath/Quaternion.h> // 四元数操作

2. 主函数流程

初始化阶段

ros::init(argc, argv, "MoveGroupInterface_To_Noetic"); // ROS节点初始化
ros::AsyncSpinner spinner(1); // 异步spinner，单线程
spinner.start(); // 启动异步线程

MoveIt!接口设置

static const std::string PLANNING_GROUP = "manipulator_i5"; // 规划组名称
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
move_group.setPoseReferenceFrame("base_link"); // 设置参考坐标系

3. 主要功能模块

回到初始位置

std::vector<double> home_position = {
-0.001255, -0.148822, -1.406503, 0.311441, -1.571295, -0.002450
};
move_group.setJointValueTarget(home_position);
move_group.move(); // 阻塞式移动到初始位置

关节空间规划

// 获取当前状态
moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
std::vector<double> joint_group_positions;
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);

// 修改关节1的角度
joint_group_positions[0] = -1.57; // 旋转90度
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);

// 可视化显示
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();

// 执行规划
move_group.execute(my_plan);

模块学习

// 修改关节1的角度
joint_group_positions[0] = -1.57; // 旋转90度

修改中括号里的数字可以改变关节