ar3：gazebo和moveit联合机械臂运动规划仿真(包含realsense视觉点云) 2024-04-30 02:31:23 0 0 1、gazebo仿真环境搭建 最终的场景： 使用的机械臂：AR3工业六轴机械臂 系统环境： ubuntu18 + ros-melodic 注：机械臂description包在github上下载的, 自己又对gazebo环境做了相应的修改。 下面是用到的主要的urdf描述文件：<?xml version="1.0" encoding="utf-8"?><!-- =================================================================================== --><!-- | This document was autogenerated by xacro from ar3.urdf | --><!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED but it was unfortunately | --><!-- =================================================================================== --><robot name="ar3" xmlns:xacro="http://www.ros.org/wiki/xacro"> <material name="SwivelWhite"> <color rgba="1.0 1.0 1.0 1"/></material> <material name="SwivelLightGray"><color rgba="0.8 0.8 0.8 1"/></material> <material name="SwivelMedGray"> <color rgba="0.6 0.6 0.6 1"/></material> <material name="SwivelDarkGray"> <color rgba="0.4 0.4 0.4 1"/></material> <material name="SwivelRed"> <color rgba="0.5 0.4 0.4 1"/></material> <material name="SwivelGreen"> <color rgba="0.4 0.5 0.4 1"/></material> <material name="SwivelBlue"> <color rgba="0.4 0.4 0.5 1"/></material> <xacro:property name = "pi" value = "3.1415927" /> <xacro:include filename="$(find realsense_ros_gazebo)/xacro/tracker.xacro"/> <xacro:include filename="$(find realsense_ros_gazebo)/xacro/depthcam.xacro"/> <xacro:realsense_d435 sensor_name="d435" parent_link="base_link" rate="10"> <origin rpy="${pi/2} ${pi/2} 0 " xyz="0 -0.35 0.7"/> </xacro:realsense_d435> <link name="world" /> <link name="base_link"> <inertial> <origin rpy="0 0 0" xyz="-4.6941E-06 0.054174 0.038824"/> <mass value="0.7102"/> <inertia ixx="0.0039943" ixy="3.697E-07" ixz="-5.7364E-08" iyy="0.0014946" iyz="-0.00036051" izz="0.0042554"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0 0 0"/> <geometry> <mesh filename="package://ar3_description/meshes/base_link.STL"/> </geometry> <material name=""> <color rgba="1 1 0 1"/> </material> </visual> <collision> <origin rpy="0 0 0" xyz="0 0 0"/> <geometry> <mesh filename="package://ar3_description/meshes/base_link.STL"/> </geometry> </collision> </link> <joint name="joint_world" type="fixed"> <parent link="world"/> <child link="base_link"/> <origin xyz="0 0 0" rpy="0 0 0" /> </joint> <link name="link_1"> <inertial> <origin rpy="0 0 0" xyz="-0.022706 0.04294 -0.12205"/> <mass value="0.88065"/> <inertia ixx="0.0034" ixy="0.00042296" ixz="-0.00089231" iyy="0.0041778" iyz="0.0010848" izz="0.0027077"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0 0 0"/> <geometry> <mesh filename="package://ar3_description/meshes/link_1.STL"/> </geometry> <material name=""> <color rgba="1 1 0 1"/> </material> </visual> <collision> <origin rpy="0 0 0" xyz="0 0 0"/> <geometry> <mesh filename="package://ar3_description/meshes/link_1.STL"/> </geometry> </collision> </link> <joint name="joint_1" type="revolute"> <origin rpy="${pi} 0 0" xyz="0 0 0.003445"/> <parent link="base_link"/> <child link="link_1"/> <axis xyz="0 0 1"/> <limit lower="-2.96706" upper="2.96706" effort="100" velocity="100"/> </joint> <link name="link_2"> <inertial> <origin rpy="0 0 0" xyz="0.064818 -0.11189 -0.038671"/> <mass value="0.57738"/> <inertia ixx="0.0047312" ixy="0.0022624" ixz="0.00032144" iyy="0.0020836" iyz="-0.00056569" izz="0.0056129"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0 0 0"/> <geometry> <mesh filename="package://ar3_description/meshes/link_2.STL"/> </geometry> <material name=""> <color rgba="1 1 0 1"/> </material> </visual> <collision> <origin rpy="0 0 0" xyz="0 0 0"/> <geometry> <mesh filename="package://ar3_description/meshes/link_2.STL"/> </geometry> </collision> </link> <joint name="joint_2" type="revolute"> <origin rpy="1.5708 0.5236 -1.5708" xyz="0 0.064146 -0.16608"/> <parent link="link_1"/> <child link="link_2"/> <axis xyz="0 0 -1"/> <limit lower="${-39.6 / 180.0 * pi}" upper="${pi / 2.0}" effort="100" velocity="100"/> </joint> <link name="link_3"> <inertial> <origin rpy="0 0 0" xyz="-0.00029765 -0.023661 -0.0019125"/> <mass value="0.1787"/> <inertia ixx="0.0001685" ixy="-2.7713E-05" ixz="5.6885E-06" iyy="0.00012865" iyz="2.9256E-05" izz="0.00020744"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0 0 0"/> <geometry> <mesh filename="package://ar3_description/meshes/link_3.STL"/> </geometry> <material name=""> <color rgba="1 1 0 1"/> </material> </visual> <collision> <origin rpy="0 0 0" xyz="0 0 0"/> <geometry> <mesh filename="package://ar3_description/meshes/link_3.STL"/> </geometry> </collision> </link> <joint name="joint_3" type="revolute"> <origin rpy="0 0 -1.04720367321" xyz="0.1525 -0.26414 0"/> <parent link="link_2"/> <child link="link_3"/> <axis xyz="0 0 -1"/> <limit lower="0.0174533" upper="2.5080381" effort="100" velocity="100"/> </joint> <link name="link_4"> <inertial> <origin rpy="0 0 0" xyz="-0.0016798 -0.00057319 -0.074404"/> <mass value="0.34936"/> <inertia ixx="0.0030532" ixy="-1.8615E-05" ixz="-7.0047E-05" iyy="0.0031033" iyz="-2.3301E-05" izz="0.00022264"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0 0 0"/> <geometry> <mesh filename="package://ar3_description/meshes/link_4.STL"/> </geometry> <material name=""> <color rgba="1 1 0 1"/> </material> </visual> <collision> <origin rpy="0 0 0" xyz="0 0 0"/> <geometry> <mesh filename="package://ar3_description/meshes/link_4.STL"/> </geometry> </collision> </link> <joint name="joint_4" type="revolute"> <origin rpy="1.5708 -1.2554 -1.5708" xyz="0 0 0.00675"/> <parent link="link_3"/> <child link="link_4"/> <axis xyz="0 0 -1"/> <limit lower="-2.8710666" upper="2.8710666" effort="100" velocity="100"/> </joint> <link name="link_5"> <inertial> <origin rpy="0 0 0" xyz="0.0015066 -1.3102E-05 -0.012585"/> <mass value="0.11562"/> <inertia ixx="5.5035E-05" ixy="-1.019E-08" ixz="-2.6243E-06" iyy="8.2921E-05" iyz="1.4437E-08" izz="5.2518E-05"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0 0 0"/> <geometry> <mesh filename="package://ar3_description/meshes/link_5.STL"/> </geometry> <material name=""> <color rgba="1 1 0 1"/> </material> </visual> <collision> <origin rpy="0 0 0" xyz="0 0 0"/> <geometry> <mesh filename="package://ar3_description/meshes/link_5.STL"/> </geometry> </collision> </link> <joint name="joint_5" type="revolute"> <origin rpy="${pi} 0 -2.8262" xyz="0 0 -0.22225"/> <parent link="link_4"/> <child link="link_5"/> <axis xyz="1 0 0"/> <limit lower="-1.81776042" upper="1.81776042" effort="100" velocity="100"/> </joint> <link name="link_6"> <inertial> <origin rpy="0 0 0" xyz="2.9287E-10 -1.6472E-09 0.0091432"/> <mass value="0.013863"/> <inertia ixx="1.3596E-06" ixy="3.0585E-13" ixz="5.7102E-14" iyy="1.7157E-06" iyz="6.3369E-09" izz="2.4332E-06"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0 0 0"/> <geometry> <mesh filename="package://ar3_description/meshes/link_6.STL"/> </geometry> <material name=""> <color rgba="1 1 0 1"/> </material> </visual> <collision> <origin rpy="0 0 0" xyz="0 0 0"/> <geometry> <mesh filename="package://ar3_description/meshes/link_6.STL"/> </geometry> </collision> </link> <joint name="joint_6" type="revolute"> <origin rpy="0 0 3.1416" xyz="-0.000294 0 0.02117"/> <parent link="link_5"/> <child link="link_6"/> <axis xyz="0 0 1"/> <limit lower="-2.5848326" upper="2.5848326" effort="100" velocity="100"/> </joint> <!-- add table --> <link name="table"> <visual> <origin xyz="0 0 0" rpy="0 0 0" /> <geometry ><box size="0.5 0.3 0.01" /> </geometry> <material name="SwivelLightGray" /> </visual> <collision> <origin xyz="0 0 0" rpy="0 0 0" /> <geometry ><box size="0.7 0.5 0.01" /> </geometry> </collision> <inertial> <mass value="9"/> <inertia ixx="9.0" ixy="0.0" ixz="0.0" iyy="9.0" iyz="0.0" izz="9.0"/> </inertial> </link> <joint name="world_table" type="fixed"> <parent link="world"/> <child link="table"/> <origin xyz="0 -0.4 0.25" rpy="0 0 0" /> </joint> <gazebo reference="table"> <material>Gazebo/LightGray</material> </gazebo> <!-- add box1 --> <link name="box1"> <visual> <origin xyz="0 0 0" rpy="0 0 0" /> <geometry ><box size="0.05 0.05 0.08" /> </geometry> <material name="SwivelRed" /> </visual> <collision> <origin xyz="0 0 0" rpy="0 0 0" /> <geometry ><box size="0.05 0.05 0.08" /> </geometry> </collision> <inertial> <mass value="9"/> <inertia ixx="9.0" ixy="0.0" ixz="0.0" iyy="9.0" iyz="0.0" izz="9.0"/> </inertial> </link> <joint name="table_box1" type="fixed"> <parent link="table"/> <child link="box1"/> <origin xyz="0 0 0.04" rpy="0 0 0" /> </joint> <gazebo reference="box1"> <material>Gazebo/Red</material> </gazebo> <!-- add box2 --> <link name="box2"> <visual> <origin xyz="0 0 0" rpy="0 0 0" /> <geometry ><box size="0.04 0.04 0.04" /> </geometry> <material name="SwivelGreen" /> </visual> <collision> <origin xyz="0 0 0" rpy="0 0 0" /> <geometry ><box size="0.04 0.04 0.04" /> </geometry> </collision> <inertial> <mass value="9"/> <inertia ixx="9.0" ixy="0.0" ixz="0.0" iyy="9.0" iyz="0.0" izz="9.0"/> </inertial> </link> <joint name="table_box2" type="fixed"> <parent link="table"/> <child link="box2"/> <origin xyz="0.1 0.05 0.025" rpy="0 0 0" /> </joint> <gazebo reference="box2"> <material>Gazebo/Green</material> </gazebo> <!-- add cylinder1 --> <link name="cylinder1"> <visual> <origin xyz="0 0 0" rpy="0 0 0" /> <geometry ><cylinder length="0.02" radius="0.03"/> </geometry> <material name="SwivelGreen" /> </visual> <collision> <origin xyz="0 0 0" rpy="0 0 0" /> <geometry ><cylinder length="0.02" radius="0.03"/> </geometry> </collision> <inertial> <mass value="9"/> <inertia ixx="9.0" ixy="0.0" ixz="0.0" iyy="9.0" iyz="0.0" izz="9.0"/> </inertial> </link> <joint name="table_cylinder1" type="fixed"> <parent link="table"/> <child link="cylinder1"/> <origin xyz="-0.1 0.05 0.015" rpy="0 0 0" /> </joint> <gazebo reference="cylinder1"> <material>Gazebo/Blue</material> </gazebo> <!-- add box3 --> <link name="box3"> <visual> <origin xyz="0 0 0" rpy="0 0 0" /> <geometry ><box size="0.02 0.02 0.02" /> </geometry> <material name="SwivelRed" /> </visual> <collision> <origin xyz="0 0 0" rpy="0 0 0" /> <geometry ><box size="0.02 0.02 0.02" /> </geometry> </collision> <inertial> <mass value="9"/> <inertia ixx="9.0" ixy="0.0" ixz="0.0" iyy="9.0" iyz="0.0" izz="9.0"/> </inertial> </link> <joint name="world_box3" type="fixed"> <parent link="world"/> <child link="box3"/> <origin xyz="0 -0.22 0.65" rpy="0 0 0" /> </joint> <gazebo reference="box3"> <material>Gazebo/Red</material> </gazebo> <transmission name="transmission_1"> <type>transmission_interface/SimpleTransmission</type> <joint name="joint_1"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> </joint> <actuator name="motor_1"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> <mechanicalReduction>1</mechanicalReduction> </actuator> </transmission> <transmission name="transmission_2"> <type>transmission_interface/SimpleTransmission</type> <joint name="joint_2"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> </joint> <actuator name="motor_2"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> <mechanicalReduction>1</mechanicalReduction> </actuator> </transmission> <transmission name="transmission_3"> <type>transmission_interface/SimpleTransmission</type> <joint name="joint_3"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> </joint> <actuator name="motor_3"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> <mechanicalReduction>1</mechanicalReduction> </actuator> </transmission> <transmission name="transmission_4"> <type>transmission_interface/SimpleTransmission</type> <joint name="joint_4"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> </joint> <actuator name="motor_4"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> <mechanicalReduction>1</mechanicalReduction> </actuator> </transmission> <transmission name="transmission_5"> <type>transmission_interface/SimpleTransmission</type> <joint name="joint_5"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> </joint> <actuator name="motor_5"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> <mechanicalReduction>1</mechanicalReduction> </actuator> </transmission> <transmission name="transmission_6"> <type>transmission_interface/SimpleTransmission</type> <joint name="joint_6"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> </joint> <actuator name="motor_6"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> <mechanicalReduction>1</mechanicalReduction> </actuator> </transmission> <gazebo> <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"> <controlPeriod>0.05</controlPeriod> <legacyModeNS>true</legacyModeNS> </plugin> </gazebo> <gazebo reference="base_link"> <selfCollide>true</selfCollide> </gazebo> <gazebo reference="link_1"> <selfCollide>true</selfCollide> </gazebo> <gazebo reference="link_2"> <selfCollide>true</selfCollide> </gazebo> <gazebo reference="link_3"> <selfCollide>true</selfCollide> </gazebo> <gazebo reference="link_4"> <selfCollide>true</selfCollide> </gazebo> <gazebo reference="link_5"> <selfCollide>true</selfCollide> </gazebo> <gazebo reference="link_6"> <selfCollide>true</selfCollide> </gazebo><!--camera--> <!-- <link name="camera_link"> <visual> <origin xyz=" 0 0 0 " rpy="${pi/2} 0 ${pi/2}" /> <geometry> <mesh filename="package://ar3_description/meshes/realsense_d435.stl"/> </geometry> <material name="black"> <color rgba="0 0 0 0.95"/> </material> </visual> </link> <joint name="camera_joint" type="fixed"> <origin xyz="0 -0.35 0.7" rpy="${pi/2} ${pi/2} 0"/> <parent link="base_link"/> <child link="camera_link"/> </joint> --> <!--gazebo--> <!-- <gazebo reference="camera_link"> <sensor type="depth" name="camera"> <update_rate>30.0</update_rate> <camera name="head"> <horizontal_fov>1.3962634</horizontal_fov> <image> <width>640</width> <height>480</height> <format>R8G8B8</format> </image> <clip> <near>0.02</near> <far>300</far> </clip> </camera> <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so"> <alwaysOn>true</alwaysOn> <updateRate>10</updateRate> <cameraName>camera</cameraName> <imageTopicName>rgb/image_raw</imageTopicName> <depthImageTopicName>depth/image_raw</depthImageTopicName> <pointCloudTopicName>depth/points</pointCloudTopicName> <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName> <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName> <frameName>camera_depth_optical_frame</frameName> <baseline>0.1</baseline> <distortion_k1>0.0</distortion_k1> <distortion_k2>0.0</distortion_k2> <distortion_k3>0.0</distortion_k3> <distortion_t1>0.0</distortion_t1> <distortion_t2>0.0</distortion_t2> <pointCloudCutoff>0.4</pointCloudCutoff> </plugin> </sensor> </gazebo> --></robot> 1.1 URDF文件解释<xacro:include filename="$(find realsense_ros_gazebo)/xacro/tracker.xacro"/> <xacro:include filename="$(find realsense_ros_gazebo)/xacro/depthcam.xacro"/> <xacro:realsense_d435 sensor_name="d435" parent_link="base_link" rate="10"> <origin rpy="${pi/2} ${pi/2} 0 " xyz="0 -0.35 0.7"/> </xacro:realsense_d435> 这一部分是引用realsense相机的urdf文件，加入到机械臂描述文件中。realsense相机描述文件在github上也可以下载。<origin rpy="${pi/2} ${pi/2} 0 " xyz="0 -0.35 0.7"/> 这条语句描述的是相机的link相对于base_link的坐标变换，即坐标是（0，-0.35，0.7），姿态是先绕着相机自身坐标系的y轴按右手定则转90度，然后绕着新的坐标系的x轴转90度。<!-- add table --> <link name="table"> <visual> <origin xyz="0 0 0" rpy="0 0 0" /> <geometry ><box size="0.5 0.3 0.01" /> </geometry> <material name="SwivelLightGray" /> </visual> <collision> <origin xyz="0 0 0" rpy="0 0 0" /> <geometry ><box size="0.7 0.5 0.01" /> </geometry> </collision> <inertial> <mass value="9"/> <inertia ixx="9.0" ixy="0.0" ixz="0.0" iyy="9.0" iyz="0.0" izz="9.0"/> </inertial> </link> <joint name="world_table" type="fixed"> <parent link="world"/> <child link="table"/> <origin xyz="0 -0.4 0.25" rpy="0 0 0" /> </joint> <gazebo reference="table"> <material>Gazebo/LightGray</material> </gazebo> 这一段的作用是在环境里添加桌子，主要包含 link , joint, gazebo说明 三块，其中joint决定桌子的位置相对world的位置，我这里也可以写成base_link，因为我这里的world和base_link坐标系是完全重合的。可以调整桌子的高度和距离机械臂的距离，不能太近，太近会撞到机械臂。 box放在桌子上作为物体，还加了一个障碍物作为机械臂moveit规划的避障物体。 1.2 启动gazebo 执行 roslaunch ar3_gazebo ar3_gazebo_bringup.launch 启动gazebo仿真环境，出现下面场景： 终端可能会报错： 这个不影响使用，可以不用管。 2、moveit配置 moveit官方教程： https://moveit.picknik.ai/galactic/doc/tutorials/quickstart_in_rviz/quickstart_in_rviz_tutorial.html 安装moveit，可以使用moveit的配置助手roslaunch moveit_setup_assistant setup_assistant.launch 官方教程也有给出。配置完成后，生成机械臂的moveit包。这里主要说一下规划组的运动学解算器的设置，在kinematic.yaml文件中：manipulator: # kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 原本的解算器是kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 改成了： kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin 有人说如果出现规划失败的问题：ABORTED: No motion plan found. No execution attempted. 就可以尝试换一下这个运动学插件。 如果换完之后编译出现缺少什么trac_ik包，安装即可。 修改默认的路径搜索算法 另外一点是在ompl_planning.yaml文件中，最后是manipulator: default_planner_config: RRTkConfigDefault planner_configs: - SBLkConfigDefault - ESTkConfigDefault - LBKPIECEkConfigDefault - BKPIECEkConfigDefault - KPIECEkConfigDefault - RRTkConfigDefault - RRTConnectkConfigDefault - RRTstarkConfigDefault - TRRTkConfigDefault - PRMkConfigDefault - PRMstarkConfigDefault - FMTkConfigDefault - BFMTkConfigDefault - PDSTkConfigDefault - STRIDEkConfigDefault - BiTRRTkConfigDefault - LBTRRTkConfigDefault - BiESTkConfigDefault - ProjESTkConfigDefault - LazyPRMkConfigDefault - LazyPRMstarkConfigDefault - SPARSkConfigDefault - SPARStwokConfigDefault projection_evaluator: joints(joint_1,joint_2) longest_valid_segment_fraction: 0.005 其中，default_planner_config: RRTkConfigDefault是我后来添加的，可以删去，这里是设置默认的路径搜索算法（我个人理解），可以设置成其他的，从下面列出的之中选择。 3、启动moveit仿真环境 执行 roslaunch ar3_moveit_config ar3_moveit_bringup_demo.launch 这里我改了一下原有的启动文件，应该也可以直接使用原来的启动文件，我修改之后的ar3_moveit_bringup_demo.launch如下：<launch> <!-- <rosparam command="load" file="$(find ar3_moveit_config)/config/joint_names.yaml" /> --> <include file="$(find ar3_moveit_config)/launch/planning_context.launch"> <arg name="load_robot_description" value="false"/> </include> <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"> <!-- <param name="/use_gui" value="true"/> --> <rosparam param="/source_list">[/joint_states]</rosparam> </node> <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) --> <include file="$(find ar3_moveit_config)/launch/move_group.launch"> <arg name="allow_trajectory_execution" value="true"/> </include> <include file="$(find ar3_moveit_config)/launch/moveit_rviz.launch"> <arg name="config" value="true"/> </include> <!-- World to base transform --> <node pkg="tf" type="static_transform_publisher" name="world_broadcaster" args = "0 0 0 0 0 0 world base_link 10" /></launch> 启动moveit,并会打开rviz 左下角是realense的rgb图像，右侧是机械臂+相机+桌子和物体+上方障碍物。 下面说一下坐标系的转换关系，这个对之后的物体坐标变换十分重要。 首先看一下world的frame和 base_link的frame,realsense的link的frame和table的frame，总共4个坐标系。 左上角是相机link的frame，左下角是桌面的frame，右下角有world和base_link的frame，两个坐标系完全重合了。坐标系的颜色 R G B对应x y z轴。 然后是深度点云坐标系和world坐标系的关系 可以发现深度点云坐标系的z轴是向下的，这里注意，之后相机发布的点云坐标，都是相对于这个 d435_depth_optical_frame的，都在这个坐标系下。因此从点云坐标，转换到base_link坐标系下，需要的过程是：点云坐标 乘以 d435_depth_optical_frame相对于base_link的变换矩阵 这一点非常重要！！ 4、利用深度相机获取目标点，并使用moveit规划路径 这一步是自己编写程序，订阅深度相机发布的点云话题，并声明一个moveit规划对象，设置目标的位置和姿态之后，使用moveit规划出路径并执行，然后gazebo中的机械臂就开始动了。 还是参考了官方文档：官方教程 官方使用了rviz自带的一个小界面，需要不断点next，我做了修改，代码如下// email:2622452304@qq.com#include <moveit/move_group_interface/move_group_interface.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_visual_tools/moveit_visual_tools.h>#include <sensor_msgs/PointCloud.h>#include <sensor_msgs/PointCloud2.h>#include <sensor_msgs/point_cloud_conversion.h>#include <ros/ros.h>#include <iostream>#include <Eigen/Core>#include <Eigen/Geometry>#define pi (3.1415926535897932346f)using namespace std;sensor_msgs::PointCloud out_pointcloud;float x;float y;float z;void pointCloud2ToZ(const sensor_msgs::PointCloud2 &msg){sensor_msgs::convertPointCloud2ToPointCloud(msg, out_pointcloud); // cout << "points_size = " << out_pointcloud.points.size() << endl; float z_min=10; for (int i=0; i<out_pointcloud.points.size(); i++) { if(out_pointcloud.points[i].y < 0.1){ if(out_pointcloud.points[i].z < z_min){ z_min = out_pointcloud.points[i].z; } } } float x_sum=0; float y_sum=0; float z_sum=0; int points_num = 20; int j = 0; for (int i=0; i<out_pointcloud.points.size(); i++){ if(out_pointcloud.points[i].y < 0.1 and out_pointcloud.points[i].z < z_min + 0.01){ if(j >= points_num){ break; } x_sum = x_sum + out_pointcloud.points[i].x; y_sum = y_sum + out_pointcloud.points[i].y; z_sum = z_sum + out_pointcloud.points[i].z; j = j+1; } } x = x_sum / points_num; y = y_sum / points_num; z = z_min;}int main(int argc, char** argv){ ros::init(argc, argv, "move2object"); ros::NodeHandle node_handle; ros::AsyncSpinner spinner(1); spinner.start(); ros::Subscriber sub = node_handle.subscribe("/d435/depth/color/points", 1, pointCloud2ToZ); // ros::Subscriber sub = node_handle.subscribe("/camera/depth/points", 1, pointCloud2ToZ); ros::Duration(1).sleep(); ros::spinOnce(); cout<< "target in camera frame is "<<" x= " << x << " y = " << y << " z= " << z <<endl; Eigen::Isometry3d T=Eigen::Isometry3d::Identity(); Eigen::AngleAxisd rotation_vector(-pi,Eigen::Vector3d(0,1,0)); // ar3_gazebo.urdf.xacro // ar3_gazebo_copy.xacro Eigen::AngleAxisd rotation_vector1; rotation_vector1 = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(-pi, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()); T.rotate(rotation_vector); T.pretranslate(Eigen::Vector3d(0, -0.35, 0.7)); Eigen::Vector3d v(x,y,z); Eigen::Vector3d v_transformed = T*v; Eigen::Vector3d ea(pi/2, 0, pi); Eigen::Quaterniond quaternion; quaternion = Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(ea[2], Eigen::Vector3d::UnitX()); cout <<"target_xyz in world frame is " << " x= " << v_transformed[0] << " y = " << v_transformed[1] << " z= " << v_transformed[2] <<endl; static const std::string PLANNING_GROUP = "manipulator"; // The :move_group_interface:`MoveGroupInterface` class can be easily // setup using just the name of the planning group you would like to control and plan for. moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP); // We will use the :planning_scene_interface:`PlanningSceneInterface` // class to add and remove collision objects in our "virtual world" scene moveit::planning_interface::PlanningSceneInterface planning_scene_interface; // Raw pointers are frequently used to refer to the planning group for improved performance. const robot_state::JointModelGroup* joint_model_group = move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP); // Getting Basic Information // We can print the name of the reference frame for this robot. ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str()); // We can also print the name of the end-effector link for this group. ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str()); geometry_msgs::Pose target_pose1; target_pose1.orientation.w = quaternion.w(); target_pose1.orientation.x = quaternion.x(); target_pose1.orientation.y = quaternion.y(); target_pose1.orientation.z = quaternion.z(); target_pose1.position.x = v_transformed[0]; target_pose1.position.y = v_transformed[1]; target_pose1.position.z = v_transformed[2]+0.02; cout << "move to traget pose1 is " <<" x= " << target_pose1.position.x<< " y = " << target_pose1.position.y << " z= " << target_pose1.position.z <<endl; move_group.setPoseTarget(target_pose1); // Now, we call the planner to compute the plan and visualize it. // Note that we are just planning, not asking move_group // to actually move the robot. moveit::planning_interface::MoveGroupInterface::Plan my_plan; bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); // ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED"); // ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line"); move_group.move(); ros::shutdown(); return 0;} 这个代码做了如下事情： 1、订阅realsense发布的点云话题，获取点云坐标； 2、遍历所有点云，找到了桌面上处于最高位置的点； 3、找出了在最高位置的点附近的点，并求这些点的平均x坐标和y坐标，以此作为最终目标的x 和y，最终目标的z还是用的是最高位置点的z； 4、把目标的(x,y,z)坐标乘以 d435_depth_optical_frame相对于base_link的变换矩阵，得到目标在base_link下的坐标； 5、手动设置目标的姿态，姿态就让末端link_6朝下，直接通过rpy姿态角度转四元数； 6、把目标的姿态和位置复赋值给 geometry_msgs::Pose target_pose1;，这里我把z加了0。02m，如果不加，可能在后面的路径规划的时候，会报错，显示无法规划出一条可行路径，原因就是目标点与其他物体非常接近，或者就在其他物体内部，不管怎么规划，机械臂最终都会和其他物体发生碰撞，所以显示规划失败，解决方法是设置合理的目标点，避免根其他物体相碰撞； 7、使用moceit规划一条路径，并执行。 视频效果： gazebo仿真动画 5.资源包链接 包含机械臂描述文件和moveit配置文件，以及机械臂运动仿真的一个demo 更新工作 1、在ar3机械臂上实现物块抓取仿真；gazbeo中抓取需要一个grasp插件，不然就会抓不起来，在此感谢博客gazebo仿真 UR10 + robotiq140抓取物体失败：滑出或滑落 和 Gazebo插件Grasp_fix介绍与踩坑 的分享。 2、抓取插件的git链接： gazebo-pkgs general-message-pkgs 这两个包的使用方法是，直接放到工作空间的 src 下，然后编译，然后在自己的机械臂描述文件 xacro 中加入插件<gazebo> <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so"> <arm> <arm_name>ur5_gripper</arm_name> <palm_link>wrist_3_link</palm_link> <gripper_link>gripper_finger1_finger_tip_link</gripper_link> <gripper_link>gripper_finger2_finger_tip_link</gripper_link> <gripper_link>gripper_finger2_knuckle_link</gripper_link> <gripper_link>gripper_finger1_knuckle_link</gripper_link> <gripper_link>gripper_finger1_inner_knuckle_link</gripper_link> <gripper_link>gripper_finger2_inner_knuckle_link</gripper_link> </arm> <forces_angle_tolerance>150</forces_angle_tolerance> <update_rate>130</update_rate> <grip_count_threshold>2</grip_count_threshold> <max_grip_count>8</max_grip_count> <release_tolerance>0.01</release_tolerance> <disable_collisions_on_attach>true</disable_collisions_on_attach> <contact_topic>__default_topic__</contact_topic> </plugin> </gazebo> 代码直接粘贴别人的，原文链接在此，主要是 libgazebo_grasp_fix.so 这个链接库。我使用的时候这个 palm_link 不能随意设置，设置的是手指的<parent_link>， <gripper_link> 就是手指的link。 3、视频 ar3机械臂抓取仿真 4、控制器使用的是 position_controller , 直接控制每个关节，由于moveit没有配置成功，所以写了一个键盘控制机械臂关节转动py代码，直接手搓位置的，后面有空再修改一下，加一个逆解包进去，感觉moveit配置稍有问题就跑不起来，最好还是自己写一个。 近期工作 UR10+gazebo+rviz吸盘仿真。博客：https://blog.csdn.net/blue_skyLZW/article/details/136098726?spm=1001.2014.3001.5502 视频： UR码垛 收藏（0）