英文解释网站
代码注释
#include <ros/ros.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
/**
* PlanningScene class: 提供了用于碰撞检查和约束检查的主要接口。在本教程中,我们将探索此类的C++接口。
*/
bool stateFeasibilityTextExample(const robot_state::RobotState& kinematic_state, bool verbose){
const double *joint_values = kinematic_state.getJointPositions("panda_joint_1");
return (joint_values[0] > 0.0);
}
int main(int argc, char **argv){
ros::init(argc, argv, "panda_arm_kinematics");
ros::AsyncSpinner spinner(1);
spinner.start();
std::size_t count = 0;
/* 使用RobotModel或者URDF和SRDF能够配置PlanningScene类,然而这不是推荐使用的方式,
推荐使用的方式是通过PlanningSceneMonitor创建和维护当前的planning scene,
该方式使用机器人的关节和机器人上的传感器(在下一教程中将详细讨论)。
但是在本教程中我们使用第一种方式,该方法仅用于说明,因为这个更简单: */
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
planning_scene::PlanningScene planning_scene(kinematic_model);
/***********************************************/
/********* 1. Self-collision checking: 检查初始状态的机器人是否自碰撞 ************/
/* 检查当前状态的机器人是否处于“自碰撞”状态,即机器人的当前配置是否会导致机器人的零件相互碰撞。
为此,我们创建CollisionRequest和CollisionResult对象,并将它们传递给碰撞检查函数。结果保存在CollisionResult中。
自碰撞检查使用的是未填充版本的机器人,即它直接使用URDF中提供的碰撞网格,而没有添加额外的填充。 */
collision_detection::CollisionRequest collision_request;
collision_detection::CollisionResult collision_result;
planning_scene.checkSelfCollision(collision_request, collision_result);
ROS_INFO_STREAM("Test 1: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");
/***********************************************/
/********* 2. Change the state: 将机器人的状态改为任意位置然后检查是否自碰撞 ************/
robot_state::RobotState& current_state = planning_scene.getCurrentStateNonConst();
current_state.setToRandomPositions();
collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result);
ROS_INFO_STREAM("Test 2: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");
/***********************************************/
/********* 3. Checking for a group: 检查机器人的手和机器人的其他部分是否碰撞 ************/
collision_request.group_name = "hand";
current_state.setToRandomPositions();
collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result);
ROS_INFO_STREAM("Test 3: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");
/***********************************************/
/********* 4. Getting Contact Information: 将机器人的关节位置设置为超出限制,然后获得碰撞信息 ************/
std::vector<double> joint_values = {0.0, 0.0, 0.0, -2.9, 0.0, 1.4, 0.0};
const robot_model::JointModelGroup* joint_model_group = current_state.getJointModelGroup("panda_arm");
current_state.setJointGroupPositions(joint_model_group, joint_values);
ROS_INFO_STREAM("Test 4: Current state is " << (current_state.satisfiesBounds(joint_model_group) ? "vaild" : "not valid"));
collision_request.contacts = true;
collision_request.max_contacts = 1000;
collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result);
ROS_INFO_STREAM("Test 5: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");
collision_detection::CollisionResult::ContactMap::const_iterator it;
for(it = collision_result.contacts.begin(); it != collision_result.contacts.end(); ++it){
ROS_INFO("Contact between: %s and %s", it->first.first.c_str(), it->first.second.c_str());
}
/***********************************************/
/********* 5. Modifying the Allowed Collision Matrix: 使用checkSelfCollision函数在ACM下检查是否自碰撞 ************/
/* ACM告诉碰撞检查器忽略掉某些部件间的碰撞,这样即使这些部件间发生了碰撞,也会被忽略掉的。
checkSelfCollision函数针对给定的ACM,检查指定状态(copied_state)是否处于自碰撞中 */
collision_detection::AllowedCollisionMatrix acm = planning_scene.getAllowedCollisionMatrix();
robot_state::RobotState copied_state = planning_scene.getCurrentState();
collision_detection::CollisionResult::ContactMap::const_iterator it2;
for(it2 = collision_result.contacts.begin(); it2 != collision_result.contacts.end(); ++it2){
/* 设置对应于一对元素的条目,最后一个参数如果为true,表示两个元素之间的碰撞是可以的,并且不需要显式的碰撞计算,
如果为false,表示必须检查两个元素之间的冲突,并且不会忽略任何冲突 */
acm.setEntry(it2->first.first, it2->first.second, true);
}
collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result, copied_state, acm);
ROS_INFO_STREAM("Test 6: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");
/***********************************************/
/********* 6. Full Collision Checking: 使用checkCollision函数在ACM下同时检查是否自碰撞和与环境的碰撞 ************/
/* checkCollision函数针对给定的ACM,检查指定状态(copied_state)是否处于碰撞中,
需要注意的是,与环境的碰撞检查将使用机器人的填充版本,填充有助于使机器人远离环境中的障碍物。 */
collision_result.clear();
planning_scene.checkCollision(collision_request, collision_result, copied_state, acm);
ROS_INFO_STREAM("Test 7: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");
/***********************************************/
/********* 7. Constraint Checking: PlanningScene class提供了检查约束的函数 ************/
/**
* 约束有两种:
* 1.从KinematicConstraint集合中选取的约束,如JointConstraint, PositionConstraint,
* OrientationConstraint和VisibilityConstraint
* 2.通过回调函数指定的用户定义的约束
*/
/* 7.1 Checking Kinematic Constraints */
std::string end_effector_name = joint_model_group->getLinkModelNames().back();
geometry_msgs::PoseStamped desired_pose;
desired_pose.pose.orientation.w = 1.0;
desired_pose.pose.position.x = 0.3;
desired_pose.pose.position.y = -0.185;
desired_pose.pose.position.z = 0.5;
desired_pose.header.frame_id = "panda_link0";
moveit_msgs::Constraints goal_constraint =
kinematic_constraints::constructGoalConstraints(end_effector_name, desired_pose);
/* 7.1.1 使用PlanningScene类中的isStateConstrained函数根据这个约束检查状态。 */
copied_state.setToRandomPositions();
copied_state.update();
bool constrained = planning_scene.isStateConstrained(copied_state, goal_constraint);
ROS_INFO_STREAM("Test 8: Random state is " << (constrained ? "constrained" : "not constrained"));
/* 7.1.2 更有效的检查约束的方法(当您想一遍又一遍地检查相同的约束时,例如在规划器内部)。
我们首先构造一个KinematicConstraintSet,对ROS Constraints消息进行预处理,并将其设置为快速处理。 */
kinematic_constraints::KinematicConstraintSet kinematic_constraints_set(kinematic_model);
kinematic_constraints_set.add(goal_constraint, planning_scene.getTransforms());
bool constrained_2 = planning_scene.isStateConstrained(copied_state, kinematic_constraints_set);
ROS_INFO_STREAM("Test 9: Random state is " << (constrained_2 ? "constrained" : "not constrained"));
/* 7.1.3 使用KinematicConstraintSet类有一种直接的方法。 */
kinematic_constraints::ConstraintEvaluationResult constraint_eval_result =
kinematic_constraints_set.decide(copied_state);
ROS_INFO_STREAM("Test 10: Random state is " << (constraint_eval_result.satisfied ? "constrained" : "not constrained"));
/* 7.2 User-defined Constraints */
/* 指定一个谓词,以决定状态在碰撞检查和约束评估所未涵盖的情况下,被视为有效还是无效。 */
planning_scene.setStateFeasibilityPredicate(stateFeasibilityTextExample);
/* 现在,每当调用isStateFeasible时,都会调用此用户定义的回调。 */
bool state_feasible = planning_scene.isStateFeasible(copied_state);
ROS_INFO_STREAM("Test 11: Random state is " << (state_feasible ? "feasible" : "not feasible"));
/* 每当调用isStateValid时,都会执行三项检查: (a)冲突检查 (b)约束检查和(c)使用用户定义的回调进行可行性检查。 */
bool state_valid = planning_scene.isStateValid(copied_state, kinematic_constraints_set, "panda_arm");
ROS_INFO_STREAM("Test 12: Random state is " << (state_valid ? "valid" : "not valid"));
/* 请注意,通过MoveIt!和OMPL可用的所有规划器当前都将使用用户定义的回调执行碰撞检查,约束检查和可行性检查。*/
ros::shutdown();
return 0;
}
运行结果
[ INFO] [1613986431.293164309]: Loading robot model 'panda'...
[ INFO] [1613986431.423661317]: Loading robot model 'panda'...
[ INFO] [1613986431.443555920]: Test 1: Current state is in self collision
[ INFO] [1613986431.443732930]: Test 2: Current state is in self collision
[ INFO] [1613986431.443845861]: Test 3: Current state is not in self collision
[ INFO] [1613986431.443870151]: Test 4: Current state is valid
[ INFO] [1613986431.443959342]: Test 5: Current state is in self collision
[ INFO] [1613986431.444000062]: Contact between: panda_leftfinger and panda_link1
[ INFO] [1613986431.444033722]: Contact between: panda_link1 and panda_rightfinger
[ INFO] [1613986431.444114823]: Test 6: Current state is not in self collision
[ INFO] [1613986431.444174623]: Test 7: Current state is not in self collision
[ INFO] [1613986431.444350274]: Test 8: Random state is not constrained
[ INFO] [1613986431.444415934]: Test 9: Random state is not constrained
[ INFO] [1613986431.444453825]: Test 10: Random state is not constrained
[ INFO] [1613986431.444484135]: Test 11: Random state is feasible
[ INFO] [1613986431.444550935]: Test 12: Random state is not valid
解释:Test6 和 Test 7添加了ACM矩阵,所以将显示没有发生自碰撞