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