moveit相关函数解释

//使用moveit需要的头文件
#include <moveit/move_group_interface/move_group.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/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>



//Move Group 界面的建立：
moveit::planning_interface::MoveGroup group("right_arm");  //建立MoveGroup类的对象group，参数是right_arm
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;  //使用PlanningSceneInterface类的对象，与世界沟通
ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);//创立一个发布者发布消息
moveit_msgs::DisplayTrajectory display_trajectory;//定义了一个相应的类的对象，用于发布消息

ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());
ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());//打印相关的消息

//设置一个目标位置
geometry_msgs::Pose target_pose1; //定义了一个Pose类的对象 target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = 0.28;
target_pose1.position.y = -0.7;
target_pose1.position.z = 1.0;
group.setPoseTarget(target_pose1);//利用group的成员函数setPoseTarget,为group类的PoseTarget变量赋值。

//现在我们利用ros进行规划，但这  ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");一步只是规划，并没有真正的让他动
moveit::planning_interface::MoveGroup::Plan my_plan;//定义一个MoveGroup类中定义的Plan类中的实体my_plan
bool success = group.plan(my_plan);//利用MoveGroup中的成员函数.plan()进行规划，成功返回true

ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");//没成功打印FAILED

sleep(5.0);//睡眠5s,等待RVIZ去可视化

//RVIZ可视化
if (1) //为啥要这么写？
{
  ROS_INFO("Visualizing plan 1 (again)");    
  display_trajectory.trajectory_start = my_plan.start_state_;
  display_trajectory.trajectory.push_back(my_plan.trajectory_);//将my_plan对应的trajectory_赋值给display_trajectory
  display_publisher.publish(display_trajectory);//将消息发出去
  sleep(5.0);
}


//规划关节空间的目标点
std::vector<double> group_variable_values;    
group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values);//将现在的关节位置向量赋值给向量group_variable_values


group_variable_values[0] = -1.0;  //改变其中的一个进行规划
group.setJointValueTarget(group_variable_values);
success = group.plan(my_plan);

//带有路径约束的规划
moveit_msgs::OrientationConstraint ocm;  
ocm.link_name = "r_wrist_roll_link";  
cm.header.frame_id = "base_link";
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.1;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 0.1;
ocm.weight = 1.0;

//把它置为路径约束
moveit_msgs::Constraints test_constraints;
test_constraints.orientation_constraints.push_back(ocm);  
group.setPathConstraints(test_constraints);

//重新设定初始状态，可为什么要这么做？
robot_state::RobotState start_state(*group.getCurrentState());
geometry_msgs::Pose start_pose2;
start_pose2.orientation.w = 1.0;
start_pose2.position.x = 0.55;
start_pose2.position.y = -0.05;
start_pose2.position.z = 0.8;
const robot_state::JointModelGroup *joint_model_group=start_state.getJointModelGroup(group.getName());//要看具体的实现细节,这里没看懂
start_state.setFromIK(joint_model_group, start_pose2);
group.setStartState(start_state);


//进行规划
group.setPoseTarget(target_pose1);
success = group.plan(my_plan);
ROS_INFO("Visualizing plan 3 (constraints) %s",success?"":"FAILED");    //这里如何实现RVIZ可视化？


//清除路径约束
group.clearPathConstraints();


//笛卡尔路径
//首先定义并waypoints
std::vector<geometry_msgs::Pose> waypoints;

geometry_msgs::Pose target_pose3 = start_pose2;
target_pose3.position.x += 0.2;
target_pose3.position.z += 0.2;
waypoints.push_back(target_pose3);  // up and out

target_pose3.position.y -= 0.2;
waypoints.push_back(target_pose3);  // left

target_pose3.position.z -= 0.2;
target_pose3.position.y += 0.2;
target_pose3.position.x -= 0.2;
waypoints.push_back(target_pose3);  // down and right (back to start)

//得到笛卡尔路径
moveit_msgs::RobotTrajectory trajectory;
double fraction = group.computeCartesianPath(waypoints,
                                               0.01,  // eef_step
                                               0.0,   // jump_threshold  //如何理解这个定义？
                                               trajectory);
ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0); //fraction 返回值的什么意思？    


//Adding/Removing Objects
//首先添加物体的信息
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = group.getPlanningFrame();//与group类建立联系


//识别物体的id
collision_object.id = "box1";


//定义一个箱子，并且给出相应的外形信息
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.4;
primitive.dimensions[1] = 0.1;
primitive.dimensions[2] = 0.4;


//箱子的位置
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x =  0.6;
box_pose.position.y = -0.4;
box_pose.position.z =  1.2;


//将上述的信息赋值给collison_object;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;


//建立一个向量，将元素压入
std::vector<moveit_msgs::CollisionObject> collision_objects;   
collision_objects.push_back(collision_object);


//将物体加入机器人的世界
ROS_INFO("Add an object into the world");  
planning_scene_interface.addCollisionObjects(collision_objects);


//设置规划的时间
group.setPlanningTime(10.0);


//进行规划
group.setStartState(*group.getCurrentState());
group.setPoseTarget(target_pose1);
success = group.plan(my_plan);


//attach or detach
//从世界中移除物体
ROS_INFO("Remove the object from the world");  
std::vector<std::string> object_ids;
object_ids.push_back(collision_object.id);  
planning_scene_interface.removeCollisionObjects(object_ids);

//双手规划
moveit::planning_interface::MoveGroup two_arms_group("arms");

two_arms_group.setPoseTarget(target_pose1, "r_wrist_roll_link");

geometry_msgs::Pose target_pose2;
target_pose2.orientation.w = 1.0;
target_pose2.position.x = 0.7;
target_pose2.position.y = 0.15;
target_pose2.position.z = 1.0;

two_arms_group.setPoseTarget(target_pose2, "l_wrist_roll_link");

moveit::planning_interface::MoveGroup::Plan two_arms_plan;
two_arms_group.plan(two_arms_plan);
sleep(4.0);




//Kinematics/C++ API
//Robotstate class and Robosmodel class

//对于RobotModel类，通常情况下，一些更高级的模块会返回指向RobotModel类的指针，我们要尽可能的妥善加以利用
//首先，我们实体化一个“RobotModelLoader”对象，他可以帮助我们查看ros参数服务器上的参数，并且创造一个 moveit_core:`RobotModel`以供使用
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel(); //注意此处，使用的即是指针
ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());



//使用moveit_core:`RobotModel`可以创造一个:moveit_core:`RobotState`，他包含了机器人的所有相关组态。我们可以将机器人的所用关节置于他的默认位置。然后我们可以得到一个:moveit_core:`JointModelGroup`，它代表了一个特定的"group"，比如说pr2的"right_arm"。
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();//各关节置为默认值，默认值在哪里？参数服务器？
const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("right_arm");

const std::vector<std::string> &joint_names = joint_model_group->getJointModelNames();



//接下来我们可以获得现在各个关节的位置
std::vector<double> joint_values;
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
for(std::size_t i = 0; i < joint_names.size(); ++i)
{
   ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
}


//关节的限制

//setJointGroupPositions()并不会强制关节限制，而是通过enforceBounds()实现的.
//接下来我们给定一个超过关节向量的值
joint_values[0] = 1.57;
kinematic_state->setJointGroupPositions(joint_model_group, joint_values);

//检查是不是有关节超出了关节限制
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));

//强制关节限制后，检查是不是超出了关节限制
kinematic_state->enforceBounds();
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));


//运动学正解
//现在，我们可以根据一系列随机的关节值来计算运动学正解
//进行计算，获得最末端关节的位置
kinematic_state->setToRandomPositions(joint_model_group);
const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("r_wrist_roll_link");//r_wrist_roll_link表示右手最末端的关节


//将位置打印出来，注意这是在robotmodel坐标系下的
ROS_INFO_STREAM("Translation: " << end_effector_state.translation());
ROS_INFO_STREAM("Rotation: " << end_effector_state.rotation());


//运动学反解
//我们可以解决pr2机器人手臂运动学反解的问题，为了完成这样的反解，我们需要以下的条件：
//1.末端执行器的位置：一就是我们以上计算出的end_effector_state
//2.解决IK问题时做出决策的数量：5
//3.每个决策的暂停时间0.1s
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 10, 0.1);//found_ik表示反解是否成功  10是什么？

//如果找到的话，我们就可以打印出IK的结论了
if (found_ik)
{
  kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
  for(std::size_t i=0; i < joint_names.size(); ++i)
  {
    ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
  }
}
else
{
  ROS_INFO("Did not find IK solution");
}

//获得Jacobian矩阵
//我们仍然从:moveit_core:`RobotState`获得Jacobian矩阵
Eigen::Vector3d reference_point_position(0.0,0.0,0.0);
Eigen::MatrixXd jacobian;
kinematic_state->getJacobian(joint_model_group, kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back()),
                             reference_point_position,
                             jacobian);
ROS_INFO_STREAM("Jacobian: " << jacobian);