【UR5机械臂Moveit避障】【6】过滤点云中UR5机械臂的部分（包围盒/滤波）【完结】

已于 2024-05-29 14:30:42 修改
阅读量855 收藏 36
点赞数 12
分类专栏： UR5机械臂Moveit避障简易实战 文章标签： python
于 2024-05-29 14:16:22 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_45702459/article/details/139294372
版权

目录

有错误或没说清楚的地方欢迎评论指正，我会修改的，谢谢大家

安装滤波ros包：

参数及launch设置

注意！

滤波参数细节

运行滤波

时间戳警告（不影响使用）

滤波微调方法

修改moveit的sensor3d中订阅中的点云话题

完成

未解决的问题

有错误或没说清楚的地方欢迎评论指正，我会修改的，谢谢大家

本来我想使用urdf精确过滤机械臂点云。但是后来我发现四个问题，主要是第四个：

1.这种滤波方法计算量太大，较简易滤波慢很多。

2.这俩相机点云波动挺大（特别是d435i），urdf滤波需要设置较大的膨胀区

3.标定导致的偏差使得膨胀区需要进一步扩大

4.我没跑通，只跑通了简易滤波

以下是urdf滤波的链接：

使用ros包：

GitHub - peci1/robot_body_filter: Filters the robot's body out of point clouds and laser scans.Filters the robot's body out of point clouds and laser scans. - peci1/robot_body_filtericon-default.png?t=N7T8https://github.com/peci1/robot_body_filter

作者给出的演示：

GitHub - ctu-vras/rosdevday_cloud_filtering: ROSDevDay 2021 Workshop on pointcloud filteringROSDevDay 2021 Workshop on pointcloud filtering. Contribute to ctu-vras/rosdevday_cloud_filtering development by creating an account on GitHub.icon-default.png?t=N7T8https://github.com/ctu-vras/rosdevday_cloud_filtering?tab=readme-ov-file

改用简易滤波：

这个robot_body_filter包确实可以根据urdf模型和坐标系过滤掉特定形状的点云，但是经过实测和作者视频中的展示，发现实时性不够，所以还是转头使用pcl的各种简单滤波吧，

上面的这个包暂时不用

安装滤波ros包：

简易滤波ros包链接：GitHub - ADVRHumanoids/point_cloud2_filters

先安装一个filter包，不然编译成功，但运行缺东西。

sudo apt-get install ros-noetic-sensor-filters

到自己的工作空间下，下载point_cloud2_filters并编译

cd ~/catkin_RealSense_ws/src 
git clone https://github.com/ADVRHumanoids/point_cloud2_filters.git
cd ..
catkin_make

确保没有报错

参数及launch设置

进入到这个包的launch中，俩相机我准备了俩launch及其yaml，各位看着来，基本就是换个名字或者话题的差别。

d435_example.launch

<launch>
    <node pkg="sensor_filters" type="pointcloud2_filter_chain" output="screen" name="pointcloud2_filters">
        <remap from="~input" to="/camera/depth/color/points" />
        <remap from="~output" to="/camera/depth/color/points/filtered" />
        <rosparam command="load" file="$(find point_cloud2_filters)/config/d435_example.yaml" />
    </node>
    
    <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" />
</launch>

k2_example.launch

<launch>
    <node pkg="sensor_filters" type="pointcloud2_filter_chain" output="screen" name="pointcloud2_filters">
        <remap from="~input" to="/kinect2/sd/points" />
        <remap from="~output" to="/kinect2/sd/points/filtered" />
        <rosparam command="load" file="$(find point_cloud2_filters)/config/k2_example.yaml" />
    </node>

    <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" />

</launch>

这个launch中，主要是把input改为相机的点云话题，output理论上可以自己定，我这里改为原points/filtered。

之后是加载yaml文件，yaml细节我下面会给到。

在最后添加一个非常强力的ros插件

<node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" />

这个到时候可以实时调节包围盒设置的参数，不需要再反复修改然后开关滤波节点。

接下来修改yaml，还是备份一下再修改：

d435_example.yaml或k2_example.yaml

output_queue_size: 1

cloud_filter_chain:
  - name: PassThroughFilter
    type: point_cloud2_filters/PassThroughFilterPointCloud2
    params:
      active: False
      input_frame: "xxxx_link"
      output_frame: "xxxx_link"
      keep_organized: True
      negative: False
      #user_filter_value: 3
      filter_field_name: "x"
      filter_limit_min: 0.5
      filter_limit_max: 0.8
      
  - name: CropBoxFilter
    type: point_cloud2_filters/CropBoxFilterPointCloud2
    params:          
      active: True
      input_frame: "xxxx_link"
      output_frame: "xxxx_link"
      keep_organized: True
      negative: False
      #user_filter_value: 5
      min_x: -1
      max_x: 1
      min_y: -2
      max_y: 2
      min_z: 0.5
      max_z: 2
      
  - name: VoxelGridFilter
    type: point_cloud2_filters/VoxelGridFilterPointCloud2
    params:
      active: True
      input_frame: "xxxx_link"
      output_frame: "xxxx_link"
      leaf_size_x: 0.01
      leaf_size_y: 0.01
      leaf_size_z: 0.01
      # min_points_per_voxel: 5
      min_points_per_voxel: 10
      downsample_all_data: True
      filter_field_name: "y"
      filter_limit_min: -2
      filter_limit_max: 2
      negative: False
      
  - name: CropBoxFilterBase
    type: point_cloud2_filters/CropBoxFilterPointCloud2
    params:          
      active: True
      input_frame: "base"
      output_frame: "xxxx_link"
      keep_organized: True
      negative: True
      #user_filter_value: 5
      min_x: -0.2
      max_x: 0.2
      min_y: -0.2
      max_y: 0.2
      min_z: 0
      max_z: 0.2

  - name: CropBoxFiltershoulder
    type: point_cloud2_filters/CropBoxFilterPointCloud2
    params:          
      active: True
      input_frame: "shoulder_link"
      output_frame: "xxxx_link"
      keep_organized: True
      negative: True
      #user_filter_value: 5
      min_x: -0.08
      max_x: 0.08
      min_y: -0.08
      max_y: 0.09
      min_z: -0.77
      max_z: 0.62

  - name: CropBoxFilterUpper
    type: point_cloud2_filters/CropBoxFilterPointCloud2
    params:          
      active: True
      input_frame: "upper_arm_link"
      output_frame: "xxxx_link"
      keep_organized: True
      negative: True
      #user_filter_value: 5
      min_x: -0.54
      max_x: 0.08
      min_y: -0.12
      max_y: 0.12
      min_z: 0.025
      max_z: 0.24

  - name: CropBoxFilterForearm
    type: point_cloud2_filters/CropBoxFilterPointCloud2
    params:          
      active: True
      input_frame: "forearm_link"
      output_frame: "xxxx_link"
      keep_organized: True
      negative: True
      #user_filter_value: 5
      min_x: -0.45
      max_x: 0.1
      min_y: -0.1
      max_y: 0.08
      min_z: -0.075
      max_z: 0.105

  - name: CropBoxFilterWrist
    type: point_cloud2_filters/CropBoxFilterPointCloud2
    params:          
      active: True
      input_frame: "wrist_1_link"
      output_frame: "xxxx_link"
      keep_organized: True
      negative: True
      #user_filter_value: 5
      min_x: -0.08
      max_x: 0.07
      min_y: -0.175
      max_y: 0.091
      min_z: -0.07
      max_z: 0.1

  - name: CropBoxFiltertool0
    type: point_cloud2_filters/CropBoxFilterPointCloud2
    params:          
      active: True
      input_frame: "tool0"
      output_frame: "xxxx_link"
      keep_organized: True
      negative: True
      #user_filter_value: 5
      min_x: -0.09
      max_x: 0.09
      min_y: -0.05
      max_y: 0.05
      min_z: -0.2
      max_z: 0.05

d435i就把xxxx_link改为camera_link,kinect2就改为kinect2_link。

注意！

这个设置只是我自己实验室的设置yaml，只是个具体点的模板，之后还是得根据读者自己的情况进行调节。

滤波参数细节

以下是yaml中注意的点：

主要用到两种滤波，

CropBoxFilter用来建立包围盒去除机械臂工作区以外的背景点云或者机械臂本体点云，

VoxelGridFilter用来暴力滤除离群的点云，防止这些点云变为体素点干扰机械臂执行。

min_points_per_voxel的值越大，离群点过滤越明显，但是也会把有用的点云变得稀疏。

变量active:True/False 开启/关闭这个滤波

变量negtive:True/False 保留盒外点云/保留盒内点云，活用active和negtive可以实时查看挖走的点云部分，进而实时调节。

滤波可以设置任意个，但是注意名字需要不同。

input_frame可以理解为基于哪个坐标系建立包围盒，所以过滤机械臂点云那里的input_frame都是关节的坐标系。

output_frame一律填自己的相机frame。

rviz的坐标系的三个箭头红绿蓝分别是xyz，有箭头的方向是正方向。

其余没说到的参数可以自己调节看看。

运行滤波

全都写好之后就可以开启节点看看是不是过滤成功tfpc.launch，详情见前节，链接，其中包含启动机器人+启动相机+运行tf变换节点

Realsense

roslaunch my_bag tfpc.launch
roslaunch point_cloud2_filters d435_example.launch

kinect2

roslaunch my_bag tfpc.launch
roslaunch point_cloud2_filters k2_example.launch

Rviz中订阅一下过滤后的点云话题，可以看见原点云（有背景）和经过滤波之后的点云（无背景）。

时间戳警告（不影响使用）

时间戳这边有点问题，但是不影响运行。尝试解决过，但是摸索了一上午，放弃了。目前已知无法解决问题的方法：ubuntu时间同步，改变tf发送频率，把tf改为tf2。

[ERROR] [1716863520.012014972]: Lookup would require extrapolation 0.011439795s into the past.  Requested time 1716863519.945887089 but the earliest data is at time 1716863519.957326889, when looking up transform from frame [camera_link] to frame [forearm_link]

滤波微调方法

先在rviz中添加tf，添加好之后觉得坐标系太多可以在tf中的frames都取消掉，只保留自己想要的

在rqt_reconfigure中可以看到所有设置的滤波，第一行调节包围盒数值。不要拖动，因为这是以米为单位的，修改右边数值，尽量以0.02为一个最小单位慢慢试。第二行active可以修改开启关闭，第三行negative可以选保留盒外还是盒里的点云，如下图就是negative开启时候的机械臂点云。

修改moveit的sensor3d中订阅中的点云话题

Ok，现在有了过滤后的点云，那么最后一步，把之前

src/universal_robot/ur5_moveit_config/config/sensors_3d.yaml中的点云话题改为过滤后的点云话题。

sensors:
  - filtered_cloud_topic: filtered_cloud
    max_range: 2.5
    max_update_rate: 5.0
    padding_offset: 0.01
    padding_scale: 1.0
    point_cloud_topic: /camera/depth/color/points/filtered
    # point_cloud_topic: /kinect2/sd/points/filtered
    # point_cloud_topic: /camera/depth/color/points
    # point_cloud_topic: /kinect2/sd/points
    point_subsample: 1
    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater

完成

至此，所有避障所需的设置就都好了，尝试一下

Realsense

roslaunch my_bag tfpc.launch
roslaunch point_cloud2_filters d435_example.launch

kinect2

roslaunch my_bag tfpc.launch
roslaunch point_cloud2_filters k2_example.launch

可以在motionplanning里驱动机械臂，也可以程序设置moveit让机械臂循环跑，具体请见链接。都是可以感知到周围环境的。

运行 demo03.py

未解决的问题

在感应到预定轨迹上有障碍物时机械臂就会停下来，我想让它停下来然后重规划。但是moveit默认是阻挡后机械臂回到起始点。目前尝试过的方法：把.go()添加一个参数wait=True，但是好像没用。可能需要编译安装moveit然后进行修改。期待后续有小伙伴可以解决。

配环境的帕鲁
关注
  • 12
    点赞
  • 踩
  • 36
    收藏
    觉得还不错? 一键收藏
  • 5
    评论
专栏目录
ROS：一节点读取发布点云，另一节点订阅点云后进行滤波处理、切割处理然后发布至ros
Skyblueee_的博客
10-10 1165
在ROS中，一节点读取发布点云，另一节点订阅点云后进行滤波处理、切割处理然后发布至ros
gazebo联合moveit机械臂运动仿真
05-21
本文将详细介绍如何利用Gazebo进行机械臂的三维仿真，并结合MoveIt!实现高级的运动规划。 **Gazebo简介** Gazebo是一款开源的多机器人仿真器，它提供了真实的物理效果和视觉渲染，可以模拟各种环境，包括室内和室外...
gazebo和moveit联合机械臂运动规划仿真(包含realsense视觉点云)
blue_skyLZW的博客
05-16 1万+
1、gazebo仿真环境搭建 最终的场景： 使用的机械臂：AR3工业六轴机械臂 系统环境： ubuntu18 + ros-melodic 注：机械臂description包在github上下载的, 自己又对gazebo环境做了相应的修改。 下面是用到的主要的urdf描述文件： <?xml version="1.0" encoding="utf-8"?> <!-- ==============================================================
Ubuntu+Ros+kinect_v2+机械臂URDF在ROS中MoveIt的配置和使用+Moveit 实时控制UR5机械臂与问题解答
weixin_72506144的博客
02-14 3703
Ubuntu+Ros+kinect_v2+UR5+Moveit
基于python的机械臂避障源码+项目说明（机械臂末端预期轨迹+主动视觉目标调度+最优感知方向决策+智能避障等）.zip
04-10
【资源说明】 1、该资源包括项目的全部源码，下载可以直接使用！ 2、本项目适合作为计算机、数学、电子信息等专业的课程设计、期末大作业和毕设项目，作为参考资料学习借鉴。 3、本资源作为“参考资料”如果需要实现其他功能，需要能看懂代码，并且热爱钻研，自行调试。 基于python的机械臂避障源码+项目说明（机械臂末端预期轨迹+主动视觉目标调度+最优感知方向决策+点云识别障碍物+滤除机械臂自身+智能避障）.zip # Active visual scheduling and intelligent obstacle avoidance ## 目录 ### PartI Questions ### PartII Quick start ### PartIII 项目说明 ## Update 注意，本次代码修改了以下部分 ``` 1. Cmakelist.txt 2. newcontroller.cpp 3. jaka/jaka_gazebo/launch/posi_only.launch 4. jaka/jaka_gazebo/launch/arm_gazebo_controller.launch 5. jaka/jaka_gazebo/config/arm_cam_joint.yaml 6. jaka/jaka_gazebo/config/arm_gazebo_control.yaml ``` 同时，cam_follow.py也有很小的修改： ``` joint1_controller -> cam_joint1_controller joint2_controller -> cam_joint2_controller ``` 现在应该使用以下代码启动控制器： ``` roslaunch jaka_gazebo posi_only.launch rosrun myController newcontroller ``` ## PartI Questions 存在已知的问题，控制器有可能卡死或者规划失败，问题根据我目前的认知来自于ros消息传递的延迟，以及重规划算法需要的时间过长。 代码很混乱，有任何问题或者改进方法都希望联系zhengzhang1002@gmail.com ## PartII Quick start ### 依赖: ubuntu 18.04 + ros melodic realsence SDK pcl(ros自带的应该就可以) ### 如何运行： 新建ros工作空间，将所有的文件夹都放在src下。 ``` mkdir -p catkin_ws/src cd catkin_ws/src catkin_init_workspace # copy files cd .. catkin_make source devel/setup.bash ``` ### 进行gazbeo仿真： ......
机械臂避障
qq_41690590的博客
02-25 412
机械臂避障
ROS下如何使用moveit驱动UR5机械臂
热门推荐
jayandchuxu的专栏
01-23 3万+
原来的moveit实在Ubuntu12.04上安装的，ros版本是hydro，后来重装电脑，系统变成了16.04，对应的ros版本为Kinetic。重装的时候，又重新记录了一下安装步骤。 moveit安装安装Users: Binary版本，因为比较简单 参考文档： Binary Installation Instructions 配置：ROS：hydro，Ubuntu：12.04 32位 1
ROS 教程6 工业机器人实战 UR5机械臂 movieit仿真 轨迹规划 Descartes 笛卡尔 轨迹规划
万有文的博客
03-05 8294
博文末尾支持二维码赞赏哦 代码ｇｉｔｈｕｂ　代码 蓝色块为仿真的工件，图示机械臂为ＵＲ５机械臂，移动到目标工件位置，对工件进行操作。 参考　ROS Industrial 仿真工件下载 sudo apt install ros-indigo-calibration-msgs cd ~/catkin_ws/src git clone https://github.com/jmey...
ROS_机械臂_个人资料.rar
03-04
来规划机械臂的运动路径，实现精确的定位和避障。 4. **传感器接口**：ROS通过各种传感器驱动包（如`camera_driver`、`laser_scan_matcher`等）收集来自摄像头、激光雷达等传感器的数据，这些数据对于机械臂的感知...
机械臂加激光雷达实现的三维环境感知避障的C++源码+文档说明
01-13
机械臂加单线激光雷达建图避障等算法研究平台 机械臂算法仿真验证平台，moveit可以直接使用。 - 不懂运行，下载完可以私聊问，可远程教学 该资源内项目源码是个人的毕设，代码都测试ok，都是运行成功后才上传资源，...
基于ROS的六自由度机械臂环境探测与避障.pdf
09-21
该方法使用RealsenseD435深度相机与机械臂构成手眼系统，通过点云数据构造OctoMap空间概率地图，在ROS环境下使用Moveit对机械臂进行运动规划，实现了机械臂在障碍物环境下的实时避障。 知识点： 1. 机械臂避障：...
panda_moveit.zip
08-23
《Panda机械臂在Gazebo与MoveIt中的配置详解》 ROS（Robot Operating System）是机器人领域广泛使用的开源操作系统，而Gazebo是一款强大的3D仿真环境，MoveIt则是ROS中的一个高级运动规划框架。当我们将这三个组件...
遨博协作机器人ROS开发 - MoveIt!设置助手机械臂配置
aubo_academy的博客
07-21 3947
大家好，欢迎关注遨博学院带来的系列技术分享文章（协作机器人ROS开发），今天我们来学习MoveIt!设置助手机械臂配置。在前面课程中，我们生成了机械臂的URDF模型文件，本节课讲解如何使用URDF文件,生成ROS系统下的MoveIt!配置功能包。...
【UR5机械臂Moveit避障】【4】UR5和kinectv2/realsense手眼标定（眼在手外）
weixin_45702459的博客
05-29 1063
我这里是眼在手外，也就是相机在固定位置，如下图所示。把frame改为base，添加点云话题，就可以看见标定成功，至于说有点偏差，那是正常的，因为这种传统的手眼标定会有一系列的多种误差累积，觉得差的可以寻找新式的标定方法（本帖末尾会给出我找到的方法），现在有点偏差，到时候建包围盒建大一点就行。reference_frame和camera_frame，填相机rgb的frame，一般在rviz测试相机的时候可以看到除了xxx_link以外还有其他frame，填带有rgb，color的关于彩色的frame。
面试题 21：解释 Python 中的 help() 函数和 dir() 函数？
最新发布
专注于全栈开发领域
07-10 747
在Python中，help()和dir()是两个非常有用的内置函数，它们可以帮助开发者更好地了解Python对象和模块。
Python28-7.3 降维算法之多维缩放MDS
Argulo的博客
07-05 683
多维尺度分析（Multidimensional Scaling, MDS）是一种用于降维的统计技术，其目标是将高维数据嵌入到低维空间中，同时尽量保持原始数据中点与点之间的距离关系。MDS 的目标是将高维数据中的点嵌入到低维（通常是二维或三维）空间中，使得在低维空间中点之间的距离尽量接近原始高维空间中点之间的距离。这通过最小化一个损失函数（通常是应力函数）来实现，应力函数衡量的是原始距离和降维后距离之间的不一致性。：对原始数据中的距离或相似度进行非度量变换，以保留原始数据中的顺序信息，而不是精确的距离值。
pandas，dataframe使用笔记
分享计算机视觉，C++，嵌入式等知识。
07-05 515
dataframe属于pandas。
java占位符替换五种方式
weixin_61478518的博客
07-08 557
在业务开发中，经常需要输出文本串。其中，部分场景下大部分文本内容是一样的，只有少部分点是不一样。简单做法是直接拼接字段，但是这会有个问题。后面如果想要修改内容，那么每个地方都要修改，这不符合设计模式中的开闭原则，面向应该对扩展开放，对修改关闭。如何解决这个问题？先来看现实生活中的例子，个人信息填写。一般会要填写表格，已经定义好了姓名，性别，年龄等字段，只要填写对应的个人信息就好了。在程序开发中，我们会预先定义好一个字符串模板，需要改动的点使用占位符。
matlab显示机械臂末端运动轨迹
05-24
要在MATLAB中显示机械臂末端的运动轨迹，需要先计算机械臂的运动学模型，然后通过将关节角度作为输入，计算机械臂末端的位置和姿态。接下来，可以使用MATLAB的绘图功能来可视化机械臂末端的运动轨迹。下面是一个简单的示例代码，以6自由度机械臂为例： ```matlab % 机械臂运动学参数 L1 = 0.2; % 关节1长度 L2 = 0.15; % 关节2长度 L3 = 0.1; % 关节3长度 L4 = 0.05; % 关节4长度 L5 = 0.15; % 关节5长度 L6 = 0.1; % 关节6长度 % 机械臂末端初始位置和姿态 T0 = [1 0 0 0.4; 0 1 0 0.2; 0 0 1 0.1; 0 0 0 1]; % 末端位姿矩阵 q0 = [0 0 0 0 0 0]; % 初始关节角度 % 计算机械臂运动轨迹 q = [q0; q0 + [0 pi/6 0 0 0 0]; q0 + [0 pi/6 pi/6 0 0 0]; q0 + [0 pi/6 pi/6 pi/6 0 0]; q0 + [0 pi/6 pi/6 pi/6 pi/6 0]; q0 + [0 pi/6 pi/6 pi/6 pi/6 pi/6]]; % 关节角度序列 T = zeros(4, 4, size(q, 1)); % 末端位姿序列 for i = 1:size(q, 1) T(:,:,i) = FK(q(i,:), L1, L2, L3, L4, L5, L6); end % 绘制机械臂运动轨迹 figure; plot3(T(1,4,:), T(2,4,:), T(3,4,:)); axis equal; xlabel('X'); ylabel('Y'); zlabel('Z'); title('机械臂末端运动轨迹'); % 正运动学函数 function T = FK(q, L1, L2, L3, L4, L5, L6) % 计算机械臂正运动学 T1 = DH(q(1), L1, 0, pi/2); T2 = DH(q(2), 0, L2, 0); T3 = DH(q(3), L3, 0, pi/2); T4 = DH(q(4), L4, 0, -pi/2); T5 = DH(q(5), 0, 0, pi/2); T6 = DH(q(6), L5+L6, 0, 0); T = T1 * T2 * T3 * T4 * T5 * T6; end % DH参数转换函数 function T = DH(theta, d, a, alpha) T = [cos(theta) -sin(theta)*cos(alpha) sin(theta)*sin(alpha) a*cos(theta); sin(theta) cos(theta)*cos(alpha) -cos(theta)*sin(alpha) a*sin(theta); 0 sin(alpha) cos(alpha) d; 0 0 0 1]; end ``` 在这个示例代码中，我们定义了机械臂的运动学参数和初始位置和姿态，然后计算了机械臂末端在不同关节角度下的位置和姿态。最后使用MATLAB的plot3函数绘制了机械臂末端的运动轨迹。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 5
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值