古月老师,我想针对您的第三个例子也就是碰撞检测的例子问几个问题。我在完成前面的机器人运动和障碍物添加之后,加入碰撞检测函数,发现robot_state::RobotState& current_state = planning_scene.getCurrentStateNonConst();并不能获取机器人现在的位置,永远是初始位置(0,0,0,0,0,0);而且例子2中添加的障碍物也没法导入进行碰撞检测,std::vector<double> joint_values = { -2.0944, 0, 0.0, 0, 0.0, 0.0 };const robot_model::JointModelGroup* joint_model_group = current_state.getJointModelGroup("manipulator");current_state.setJointGroupPositions(joint_model_group, joint_values);(以上是用ur5的joint_values编辑的与例子2中的障碍物有碰撞的关节位置),检测结果为无碰撞,另外我还发现用current_state.setJointGroupPositions(joint_model_group, joint_values);修改机械臂关节位置,其各关节DH参数不会改变(打印出 ROS_INFO_STREAM(current_state);发现的)所以,想问这个planning_sceneAPI是不是需要特殊的声明才能获得当前真实环境?#include <ros/ros.h>
// 包含moveit的API
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <eigen_conversions/eigen_msg.h>
#include <vector>
#include <iostream>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
// std::vector<std::string> joint_names {"shoulder_pan_joint","shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint","wrist_3_joint"};
int main(int argc, char **argv)
{
ros::init (argc, argv, "arm_kinematics");
ros::AsyncSpinner spinner(1);
spinner.start();
// 创建运动规划的情景,等待创建完成
moveit::planning_interface::MoveGroupInterface arm("manipulator");
moveit::planning_interface::PlanningSceneInterface current_scene;
sleep(5.0);
// 声明一个障碍物的实例,并且为其设置一个id,方便对其进行操作,该实例会发布到当前的情景实例中
moveit_msgs::CollisionObject cylinder;
cylinder.header.frame_id = arm.getPlanningFrame();
cylinder.id = "seven_dof_arm_cylinder";
// 设置障碍物的外形、尺寸等属性
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.CYLINDER;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.6;
primitive.dimensions[1] = 0.2;
// 设置障碍物的位置
geometry_msgs::Pose pose;
pose.orientation.w = 1.0;
pose.position.x = 0.0;
pose.position.y = -0.4;
pose.position.z = 0.4;
// 将障碍物的属性、位置加入到障碍物的实例中
cylinder.primitives.push_back(primitive);
cylinder.primitive_poses.push_back(pose);
cylinder.operation = cylinder.ADD;
// 创建一个障碍物的列表,把之前创建的障碍物实例加入其中
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(cylinder);
// 所有障碍物加入列表后(这里只有一个障碍物),再把障碍物加入到当前的情景中,如果要删除障碍物,使用removeCollisionObjects(collision_objects)
current_scene.addCollisionObjects(collision_objects);
//move
arm.setGoalJointTolerance(0.001);
arm.setMaxAccelerationScalingFactor(0.2);
arm.setMaxVelocityScalingFactor(0.2);
// 控制机械臂先回到初始化位置
arm.setNamedTarget("up");
arm.move();
sleep(1);
// 随机产生一个目标位置
arm.setRandomTarget();
// 开始运动规划,并且让机械臂移动到目标位置
arm.move();
sleep(1);
// 加载机器人的运动学模型到情景实例中
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);
// 自身碰撞检测
// 首先需要创建一个碰撞检测的请求对象和响应对象,然后调用碰撞检测的API checkSelfCollision,检测结果会放到collision_result中
collision_detection::CollisionRequest collision_request;
collision_detection::CollisionResult collision_result;
// 修改机器人的状态
// 我们可以使用场景实例的getCurrentStateNonConst()获取当前机器人的状态,然后修改机器人的状态到一个随机的位置,
// 清零collision_result的结果后再次检测机器人是否发生滋生碰撞
robot_state::RobotState& current_state = planning_scene.getCurrentStateNonConst();
ROS_INFO_STREAM(current_state);
// current_state.setToRandomPositions();
// 获取碰撞关系
// 首先,我们先让机器人发生自身碰撞
std::vector<double> joint_values = { -2.0944, 0, 0.0, 0, 0.0, 0.0 };
const robot_model::JointModelGroup* joint_model_group = current_state.getJointModelGroup("manipulator");
current_state.setJointGroupPositions(joint_model_group, joint_values);
ROS_INFO_STREAM(current_state);
// 修改允许碰撞矩阵(Allowed Collision Matrix,acm)
// 我们可以通过修改acm来指定机器人是否检测自身碰撞和与障碍物的碰撞,在不检测的状态下,即使发生碰撞,检测结果也会显示未发生碰撞
collision_request.contacts = true;
collision_request.max_contacts = 1000;
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)
{
acm.setEntry(it2->first.first, it2->first.second, true);
}
// 完整的碰撞检测
// 同时检测自身碰撞和与障碍物的碰撞
collision_result.clear();
planning_scene.checkCollision(collision_request, collision_result, copied_state, acm);
ROS_INFO_STREAM("6. Full collision Test: "<< (collision_result.collision ? "in" : "not in")
<< " collision");
ros::shutdown();
return 0;
}
展示全文
第三方账号登入
QQ 微博 微信