movit 相关函数(二)

//The kinematics.yaml file 运动学参数文件
//该文件是由MoveIt! Setup Assistant生成的记录初始组态的。
right_arm:
  kinematics_solver: pr2_arm_kinematics/PR2ArmKinematicsPlugin
  kinematics_solver_search_resolution: 0.001
  kinematics_solver_timeout: 0.05
  kinematics_solver_attempts: 3
right_arm_and_torso:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05

//kinematics_solver:是运动学处理插件的名字,应该和你在插件描述文件(plugin description file)中的声明是一样的。
//kinematics_solver_search_resolution: 声明了分辨率,sovler可能用它来搜索运动学反解的不可达空间。??
//kinematics_solver_timeout:运动学反解每个内部循环的延迟。
//kinematics_solver_attempts: 反解中restarts的个数,关节个数?

//KDL运动学插件:封装的运动学反解包

//Position Only IK
position_only_ik: True//在描述文件中加入下列语句即可声明,用来做什么的?

//Planning Scene/C++ API  C++API中的PlanningScene类,提供了用户碰撞检查和约束检查的界面

//这样的实例化PlanningScene的方式并不推荐,这里只是简单的介绍
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);//使用RobotModel类初始化PlanningScene类

//碰撞检查
 
//自碰撞检查
//我们要做的第一件事是检查现在机器人的组态会不会导致机器人的部分相互碰撞。
collision_detection::CollisionRequest collision_request;//实例化一个CollisionRequest对象
collision_detection::CollisionResult collision_result;//实例化一个CollisionResult对象
planning_scene.checkSelfCollision(collision_request, collision_result);//送入checkSelfCollison函数检查
ROS_INFO_STREAM("Test 1: Current state is "
                << (collision_result.collision ? "in" : "not in")
                << " self collision");//通过collision_result.collision反应出来

//改变机器人的状态
robot_state::RobotState& current_state = planning_scene.getCurrentStateNonConst();//从PlanningScene类中获得机器人的当前信息
current_state.setToRandomPositions();//随机修改当前信息?
collision_result.clear();//修改信息后,需要调用.clear()才能进行判断
planning_scene.checkSelfCollision(collision_request, collision_result);
ROS_INFO_STREAM("Test 2: Current state is "
                << (collision_result.collision ? "in" : "not in")
                << " self collision");//对于新状态的自碰撞判断

//对于机器人一个“group”检查
collision_request.group_name = "right_arm";//制定要检查的group为“right_arm”
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");//进行碰撞判断

//Getting Contact Information 获得触点信息
//我们手动的将"right_arm"推到一个会碰撞的点,并会检查是不是超出关节限制
std::vector<double> joint_values;
const robot_model::JointModelGroup* joint_model_group =
  current_state.getJointModelGroup("right_arm");
current_state.copyJointGroupPositions(joint_model_group, joint_values);
joint_values[0] = 1.57; //hard-coded since we know collisions will happen here
current_state.setJointGroupPositions(joint_model_group, joint_values);
ROS_INFO_STREAM("Current state is "
                << (current_state.satisfiesBounds(joint_model_group) ? "valid" : "not valid"));

//现在我们可以获得“right_arm”可能出现的接触点的信息,
collision_request.contacts = true;
collision_request.max_contacts = 1000;

collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result);
ROS_INFO_STREAM("Test 4: 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());
}//这个contact是什么鬼?

//修改“允许碰撞模型”“the Allowed Collision Matrix”
//ACM提供了一种在“碰撞模型的世界”下忽略与某些物体(其中包括了机器人本身和外界世界)碰撞的机制。
//接下来我们将会复制“the Allowed Collision Matrix”和“the current state”,并将他们传递给碰撞检查的函数
ollision_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.checkSelfCollision(collision_request, collision_result, copied_state, acm);//检查判断
ROS_INFO_STREAM("Test 5: Current state is "
                << (collision_result.collision ? "in" : "not in")
                << " self collision");//这里还是没看懂在干什么

//全局碰撞检查
//copied_state携带机器人信息,acm携带了环境的信息
collision_result.clear();
planning_scene.checkCollision(collision_request, collision_result, copied_state, acm);
ROS_INFO_STREAM("Test 6: Current state is "
                << (collision_result.collision ? "in" : "not in")
                << " self collision");

//约束检查
//PlanningScene类也提供了方便进行约束检查的调用函数。
//约束一般分为两类,(1)运动学约束:比如说关节约束,位置约束,方向约束和能见度约束  (2)使用者通过回调声明的约束
//检查运动学约束
//We will first define a simple position and orientation constraint on the end-effector of the right_arm of the PR2 robot.
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.75;
desired_pose.pose.position.y = -0.185;
desired_pose.pose.position.z = 1.3;
desired_pose.header.frame_id = "base_footprint";
moveit_msgs::Constraints goal_constraint =
  kinematic_constraints::constructGoalConstraints(end_effector_name, desired_pose);

//检查这个约束
copied_state.setToRandomPositions();
copied_state.update();//什么鬼?
bool constrained = planning_scene.isStateConstrained(copied_state, goal_constraint);//以bool来表示新的位置的规划是否与这个约束冲突。
ROS_INFO_STREAM("Test 7: Random state is "
                << (constrained ? "constrained" : "not constrained"));

//还有一个更加高效的检查约束的方式(比如说:当你一次又一次的检查同一个约束时,考虑一个planner);
//我们首先建立一个KinematicConstraintSet,它可以预处理ROS约束,并且快速建立这个约束。
kinematic_constraints::KinematicConstraintSet kinematic_constraint_set(kinematic_model);
kinematic_constraint_set.add(goal_constraint, planning_scene.getTransforms());//定义类并调用函数
bool constrained_2 =
  planning_scene.isStateConstrained(copied_state, kinematic_constraint_set);//检查约束
ROS_INFO_STREAM("Test 8: Random state is "
                << (constrained_2 ? "constrained" : "not constrained"));

//使用KinematicConstraintSet类可以直接完成这个行动
kinematic_constraints::ConstraintEvaluationResult constraint_eval_result =
  kinematic_constraint_set.decide(copied_state);
ROS_INFO_STREAM("Test 9: Random state is "
                << (constraint_eval_result.satisfied ? "constrained" : "not constrained"));

//用户定义的约束
bool userCallback(const robot_state::RobotState &kinematic_state, bool verbose)
{
  const double* joint_values = kinematic_state.getJointPositions("r_shoulder_pan_joint");
  return (joint_values[0] > 0.0);
}//这个简单的回调函数检测关节是不是活跃的

planning_scene.setStateFeasibilityPredicate(userCallback);
bool state_feasible = planning_scene.isStateFeasible(copied_state);
ROS_INFO_STREAM("Test 10: Random state is "
                << (state_feasible ? "feasible" : "not feasible"));

bool state_valid =
  planning_scene.isStateValid(copied_state, kinematic_constraint_set, "right_arm");
ROS_INFO_STREAM("Test 10: Random state is "
                << (state_valid ? "valid" : "not valid"));//代码没太看懂

//Planning Scene/ROS API
//Adding and removing objects into the world
//Attaching and detaching objects to the robot

//ROS API
//利用ROS API得到一个planning scene publisher是通过一个使用“diff”的话题界面,这也是这与之前的不同之处。

//声明一个必要的
ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
while(planning_scene_diff_publisher.getNumSubscribers() < 1)//与这个topic相连的用户的个数
{
  ros::WallDuration sleep_t(0.5);
  sleep_t.sleep();//休眠0.5s
}

//定义the attached object消息
//我们将会使用这个消息从世界中加减物体,以及“attach the object to the robot”
moveit_msgs::AttachedCollisionObject attached_object;
attached_object.link_name = "r_wrist_roll_link";
/* The header must contain a valid TF frame*/
attached_object.object.header.frame_id = "r_wrist_roll_link";
/* The id of the object */
attached_object.object.id = "box";

/* A default pose */
geometry_msgs::Pose pose;
pose.orientation.w = 1.0;//为何不定义x,y,z?

/* Define a box to be attached */
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.1;
primitive.dimensions[1] = 0.1;
primitive.dimensions[2] = 0.1;

attached_object.object.primitives.push_back(primitive);
attached_object.object.primitive_poses.push_back(pose);

//完成attach the objiect to the robot操作,opreation要制成ADD
attached_object.object.operation = attached_object.object.ADD;

//把物体加入环境,之前的怎么算?
ROS_INFO("Adding the object into the world at the location of the right wrist.");
moveit_msgs::PlanningScene planning_scene;
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);
sleep_time.sleep();

//Attach an object to the robot
//当机器人从环境中抓取物体时,我们需要“attach an object to the robot”,任何有关于机器人模型的操作都会考虑到这个物体

//Attach an object to the robot需要两部操作
//1.将物体从世界中移除
//2.将物体加入机器人模型

//首先,定义移除物体信息
moveit_msgs::CollisionObject remove_object;
remove_object.id = "box";
remove_object.header.frame_id = "odom_combined";
remove_object.operation = remove_object.REMOVE;//移除物体的操作,opreation定义成remove

ROS_INFO("Attaching the object to the right wrist and removing it from the world.");
planning_scene.world.collision_objects.clear();//首先确保这个话题不和其他的物体相关联
planning_scene.world.collision_objects.push_back(remove_object);//从世界移除物体
planning_scene.robot_state.attached_collision_objects.push_back(attached_object);//加入机器人
planning_scene_diff_publisher.publish(planning_scene);//发布出相应的消息

sleep_time.sleep();

//Detach an object from the robot
//上述操作分为两步
//1.从机器人移除物体
//2.重新加回世界
moveit_msgs::AttachedCollisionObject detach_object;
detach_object.object.id = "box";
detach_object.link_name = "r_wrist_roll_link";
detach_object.object.operation = attached_object.object.REMOVE;

ROS_INFO("Detaching the object from the robot and returning it to the world.");
planning_scene.robot_state.attached_collision_objects.clear();
planning_scene.robot_state.attached_collision_objects.push_back(detach_object);
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene_diff_publisher.publish(planning_scene);

sleep_time.sleep();//整体与之上是一致的

//将物体移除世界
ROS_INFO("Removing the object from the world.");
planning_scene.robot_state.attached_collision_objects.clear();
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(remove_object);
planning_scene_diff_publisher.publish(planning_scene);

//运动规划 C++ API
//我们将用C++代码完成运动规划

//start
//在开始规划之前,我们需要两样东西
//1.机器人模型(RobotModel class)
//2.规划现场的信息(PlanningScene class)

//首先创建一个robot model
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();

//使用这个机器人模型,我们可以创建一个PlanningScene对象
planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));

//现在,我们要创建一个loader,用于加载一个规划
boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader;  //规划插件loader
planning_interface::PlannerManagerPtr planner_instance; //规划实例
std::string planner_plugin_name;//规划插件名字

//我们将要获得从参数服务器加载规划的名称,然后加载这个规划并且确保包括了所有的信息
if (!node_handle.getParam("planning_plugin", planner_plugin_name))
  ROS_FATAL_STREAM("Could not find planner plugin name");
try
{
  planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>("moveit_core", "planning_interface::PlannerManager"));
}
catch(pluginlib::PluginlibException& ex)
{
  ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
}
try
{
  planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
  if (!planner_instance->initialize(robot_model, node_handle.getNamespace()))
    ROS_FATAL_STREAM("Could not initialize planner instance");
  ROS_INFO_STREAM("Using planning interface ‘" << planner_instance->getDescription() << "‘");
}
catch(pluginlib::PluginlibException& ex)
{
  const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses();
  std::stringstream ss;
  for (std::size_t i = 0 ; i < classes.size() ; ++i)
    ss << classes[i] << " ";
  ROS_ERROR_STREAM("Exception while loading planner ‘" << planner_plugin_name << "‘: " << ex.what() << std::endl
                   << "Available plugins: " << ss.str());
}

//目标位置
planning_interface::MotionPlanRequest req;
planning_interface::MotionPlanResponse res;
geometry_msgs::PoseStamped pose;
pose.header.frame_id = "torso_lift_link";
pose.pose.position.x = 0.75;
pose.pose.position.y = 0.0;
pose.pose.position.z = 0.0;
pose.pose.orientation.w = 1.0;

//设置位置和角度的允差0.01m和0.01rad
std::vector<double> tolerance_pose(3, 0.01);
std::vector<double> tolerance_angle(3, 0.01);

//我们将会利用kinematic_constraints包里提供的函数创建一个请求
req.group_name = "right_arm";
moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("r_wrist_roll_link", pose, tolerance_pose, tolerance_angle);
req.goal_constraints.push_back(pose_goal);//利用pose_goal创建了一个req

//我们将会创建一个规划context,它将现场,请求和相应封装起来。我们通过这个context来调用这个planner
planning_interface::PlanningContextPtr context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
context->solve(res);
if(res.error_code_.val != res.error_code_.SUCCESS)
{
  ROS_ERROR("Could not compute plan successfully");
  return 0;
}

//关节值的目标
//首先把规划现场设置为上一个规划的末状态
robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst();
planning_scene->setCurrentState(response.trajectory_start);
const robot_state::JointModelGroup* joint_model_group = robot_state.getJointModelGroup("right_arm");
robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);

//建立一个关节空间的目标
robot_state::RobotState goal_state(robot_model);
std::vector<double> joint_values(7, 0.0);
joint_values[0] = -2.0;
joint_values[3] = -0.2;
joint_values[5] = -0.15;
goal_state.setJointGroupPositions(joint_model_group, joint_values);
moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);
req.goal_constraints.clear();
req.goal_constraints.push_back(joint_goal);//利用这个目标简历了一个请求

//调用这个规划
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
/* Call the Planner */
context->solve(res);
/* Check that the planning was successful */
if(res.error_code_.val != res.error_code_.SUCCESS)
{
  ROS_ERROR("Could not compute plan successfully");
  return 0;
}
/* Visualize the trajectory */
ROS_INFO("Visualizing the trajectory");
res.getMessage(response);
display_trajectory.trajectory_start = response.trajectory_start;
display_trajectory.trajectory.push_back(response.trajectory);

/* Now you should see two planned trajectories in series*/
display_publisher.publish(display_trajectory);

/* We will add more goals. But first, set the state in the planning
   scene to the final state of the last plan */
robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);

/* Now, we go back to the first goal*/
req.goal_constraints.clear();
req.goal_constraints.push_back(pose_goal);
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
context->solve(res);
res.getMessage(response);//标准方式
display_trajectory.trajectory.push_back(response.trajectory);
display_publisher.publish(display_trajectory);

//添加路径约束
//新添加一个目标位置,并且添加路径约束
/* Let‘s create a new pose goal */
pose.pose.position.x = 0.65;
pose.pose.position.y = -0.2;
pose.pose.position.z = -0.1;
moveit_msgs::Constraints pose_goal_2 = kinematic_constraints::constructGoalConstraints("r_wrist_roll_link", pose, tolerance_pose, tolerance_angle);

/* First, set the state in the planning scene to the final state of the last plan */
robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
/* Now, let‘s try to move to this new pose goal*/
req.goal_constraints.clear();
req.goal_constraints.push_back(pose_goal_2);

geometry_msgs::QuaternionStamped quaternion;
quaternion.header.frame_id = "torso_lift_link";
quaternion.quaternion.w = 1.0;
req.path_constraints = kinematic_constraints::constructGoalConstraints("r_wrist_roll_link", quaternion);//在req中设置了路径约束

//工作空间的限制
req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y = req.workspace_parameters.min_corner.z = -2.0;
req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y = req.workspace_parameters.max_corner.z =  2.0;

//call the planner
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
context->solve(res);
res.getMessage(response);
display_trajectory.trajectory.push_back(response.trajectory);
display_publisher.publish(display_trajectory);//使用rviz观看结果

时间: 2024-10-01 03:29:09

movit 相关函数(二)的相关文章

(转载)(收藏)Awk学习详细文档

awk命令 本文索引 [隐藏] awk命令格式和选项 awk模式和操作 模式 操作 awk脚本基本结构 awk的工作原理 awk内置变量(预定义变量) 将外部变量值传递给awk awk运算与判断 算术运算符 赋值运算符 逻辑运算符 正则运算符 关系运算符 其它运算符 运算级优先级表 awk高级输入输出 读取下一条记录 简单地读取一条记录 关闭文件 输出到一个文件 设置字段定界符 流程控制语句 条件判断语句 循环语句 while语句 for循环 do循环 其他语句 数组应用 数组的定义 数组相关函

AWK&amp;SED

目录 命令实例 1.显示两个目录中不同的文件 2.打补丁 3.找出两个文件相同的记录 4.找不同的记录 5.对文件的某一列进行统计 6.将c文件中第一列放到到d文件中的第三列 7.删除重复行,顺序不变 uniq 选项 参数 实例 sort命令 语法 选项 参数 实例 du find awk awk命令格式和选项 awk模式和操作 模式 操作 awk脚本基本结构 awk的工作原理 awk内置变量(预定义变量) 重点 将外部变量值传递给awk awk运算与判断 算术运算符 赋值运算符 逻辑运算符 正

python-1:Number数字类型 之二 相关函数 int.from_bytes,int.to_bytes()

函数格式:int.from_bytes(bytes, byteorder, *, signed=False) 简单demo: [python] view plain copy <code class="language-python">s1 = b'\xf1\xff' print(int.from_bytes(s1, byteorder='big', signed=False)) print(int.from_bytes(s1, byteorder='little', si

学习笔记(2)---Matlab 图像处理相关函数命令大全

Matlab 图像处理相关函数命令大全 一.通用函数: colorbar  显示彩色条 语法:colorbar \ colorbar('vert') \ colorbar('horiz') \ colorbar(h) \ h=colorbar(...) \ colorbar(...,'peer',axes_handle) getimage 从坐标轴取得图像数据 语法:A=getimage(h) \ [x,y,A]=getimage(h) \ [...,A,flag]=getimage(h) \

dedecms二次开发:dedesql.class.php 数据库类

dedecms二次开发目录点这个:dedecms二次开发教程目录 系统会自动载入 dedesql.class.php 文件,并用 $dsql = $db = new DedeSql(false); 进行初始化数据库连接,因此在工程所有文件中均不需要单独初始化这个类,可直接用 $dsql 或 $db 进行操作,为了防止错误,操作完后不必关闭数据库. 常用的方法: 1.执行一个非查询类型的SQL语句,如 insert .create .update 等 $rs = $db->ExecuteNoneQ

生成二维码

PHPqrCode是一个PHP二维码生成类库,利用它可以轻松生成二维码,官网提供了下载和多个演示demo, 查看地址:http://phpqrcode.sourceforge.net/.    下载官网提供的类库后,只需要使用phpqrcode.php就可以生成二维码了,当然您的PHP环境必须开启支持GD2. phpqrcode.php提供了一个关键的png()方法,其中参数$text表示生成二位的的信息文本:参数$outfile表示是否输出二维码图片 文件,默认否:参数$level表示容错率,

使用PHP QRCode类库生成二维码

QRCode是一个PHP二维码生成类库,利用它可以轻松生成二维码. 下载后,只需要使用phpqrcode.php就可以生成二维码了,当然您的PHP环境必须开启支持GD2. phpqrcode.php提供了一个关键的png()方法,其中: 参数$text表示生成二位的的信息文本; 参数$outfile表示是否输出二维码图片 文件,默认否; 参数$level表示容错率,也就是有被覆盖的区域还能识别,分别是L(QR_ECLEVEL_L,7%),M(QR_ECLEVEL_M,15%),Q(QR_ECLE

20150411--Dede二次开发-01

目录 一.目前市场流行的电子商城系统 1 二.ecshop的介绍 1 三.安装 2 四.echsop 的目录结构 5 五.分析ecshop里面程序的架构 5 六.小试牛刀把面包屑导航改成两个大于号 6 1.根据php页面找出该页面对应的模板. 6 2.找到category.dwt模板文件,打开找到面包屑导航的位置 6 3.在library目录中找出  ur_here.lbi文件 7 七.init.php文件分析 9 八.完成在用户登录是, 可以使用邮箱登录. 11 九.完成余额显示: 15 十.

使用PHP QR Code生成二维码

HP QR Code是一个PHP二维码生成类库,利用它可以轻松生成二维码,官网提供了下载和多个演示demo,查看地址: http://phpqrcode.sourceforge.net/ 下载官网提供的类库后,只需要使用phpqrcode.php就可以生成二维码了,当然您的PHP环境必须开启支持GD2. phpqrcode.php提供了一个关键的png()方法,其中 参数$text表示生成二位的的信息文本: 参数$outfile表示是否输出二维码图片 文件,默认否: 参数$level表示容错率,