《鸿蒙机器人比赛》课件-06 Moveit编程接口

MoveIt!编程接口课程内容1.MoveIt!的编程接口2.关节空间运动规划3.笛卡尔空间运动规划4.碰撞检测5.运动学插件的配置2©NXROBO20231.MoveIt的编程接口3©NXROBO2023ROS参数服务器A

ction用户接口机器人MoveGroupA

ctionPick关节轨迹move_

group_

i

nterf

ace机器人控制器Place（C

++接口）Servi

ce笛卡尔路径逆向运动学正向运动学规划有效性路径规划Topicmovei

t_

commander（Py

thon接口）点云机器人3D传感器关节状态机器人传感器路径执行GUI规划场景（rviz插件接口）附着物体其他接口碰撞物体TF机器人状态发布规划场景差异MoveIt!的核心节点——move_group1.MoveIt的编程接口——C++&Python4©NXROBO2023PythonC++1.MoveIt的编程接口——接口实现流程5©NXROBO20231.MoveIt的编程接口——官方API和基础教程6©NXROBO2023https://ros-planning.github.io/moveit_tutorials/1.MoveIt的编程接口7©NXROBO2023运动规划程序步骤：1）连接控制需要的规划组2）设置目标位姿（关节空间或笛卡尔空间）

3）设置运动约束（可选）4）使用MoveIt!规划一条到达目标的轨迹5）修改轨迹（如速度等参数）6）执行规划出的轨迹2.关节空间运动规划——正向运动学8©NXROBO2023点到点运动：规划关节空间中起始状态到终止状态的轨迹，不考虑机械臂末端的轨迹起始状态终止状态2.关节空间运动规划——正向运动学9©NXROBO202310©NXROBO20232.关节空间运动规划——正向运动学arm=moveit_commander.MoveGroupCommander('arm')

joint_positions=[-0.0867,-1.274,0.02832,0.0820,-1.273,-0.003]arm.set_joint_value_target(joint_positions)

arm.go()

gripper=moveit_commander.MoveGroupCommander('gripper')gripper.set_joint_value_target([0.01])gripper.go()关键API调用的步骤1）创建规划组的控制对象；2）设置关节空间运动的目标位姿；3）运动规划并控制机械臂运动。机械臂规划组夹爪规划组11©NXROBO20232.关节空间运动规划——正向运动学$roslaunch

marm_moveit_configdemo.launch$rosrun

marm_planning

moveit_fk_demo.py运行如下命令：运动效果：机械臂回到原位(home位置)夹子收紧机械臂运动到另一个位置12©NXROBO20232.关节空间运动规划——逆向运动学13©NXROBO20232.关节空间运动规划——逆向运动学arm=moveit_commander.MoveGroupCommander('arm')reference_frame='base_link'

arm.set_pose_reference_frame(reference_frame)

arm.set_start_state_to_current_state()

target_pose=…

end_effector_link=arm.get_end_effector_link()

arm.set_pose_target(target_pose,end_effector_link)

traj=arm.plan()

arm.execute(traj)关键API调用的步骤1）创建规划组的控制对象；2）设置参考坐标系；3）设置机械臂起始状态；4）设置末端执行器的目标位姿；5）运动规划计算；6）执行控制机械臂运动。机械臂规划组运行如下命令：运动效果：机械臂回到原位(home位置)机械臂运动到另一个位置14©NXROBO20232.关节空间运动规划——逆向运动学$roslaunch

marm_moveit_configdemo.launch$rosrun

marm_planning

moveit_ik_demo.py15©NXROBO20233.笛卡尔空间运动规划目标状态设置：1)用关节角表达；2)用末端执行器位姿表达不考虑中间状态的约束，如不关心末端的轨迹关节空间的运动规划16©NXROBO20233.笛卡尔空间运动规划笛卡尔路径约束，路径点之间的路径形状是一条直线。17©NXROBO2023$

roslaunchmarm_moveit_configdemo.launch工作空间规划例程$

rosrunmarm_planning

moveit_cartesian_demo.py

_cartesian:=True（走直线）$

rosrunmarm_planning

moveit_cartesian_demo.py

_cartesian:=False（走曲线）3.笛卡尔空间运动规划注意：1)到网上下载功能包robot_marm，放到工作空间的src目录下，并编译此包；2)需将所有的python文件的权限设置为“可执行”；3)若ros版本为noetic，还需要将python文件的第一行的python改为python3。18©NXROBO20233.笛卡尔空间运动规划19©NXROBO20233.笛卡尔空间运动规划20©NXROBO20233.笛卡尔空间运动规划关键API的实现步骤返回值

plan：规划出来的运动轨迹

fraction：描述规划成功的轨迹在给定路点列表中的覆盖率[0~1]。如果fraction小于1，说明给定的路点列表没办法完整规划。21©NXROBO20234.碰撞检测MoveIt!可以在运动规划时检测碰撞，并规划轨迹绕过障碍22©NXROBO20234.碰撞检测——碰撞检测算法碰撞检测是运动规划中最耗时的运算之一，往往会占用90%左右的时间为了降低计算量，可在Moveit!SetupAssistant工具中设置碰撞检测矩阵(ACM,AllowedCollisionMatrix)进行优化Moveit!使用CollisionWorld对象进行碰撞检测采用FCL(FlexibleCollisionLibrary)或PCD(ProximityCollisionDetection)功能包实现；23©NXROBO20234.碰撞检测——规划场景监听信息用户接口机器人Topic•

状态信息点云move_group_interface（C++）接口机器人3D传感器（State

Information）机器人的关节话题joint_states；深度图像附着物体碰撞物体moveit_commander（Python接口）关节状态规划场景差异机器人传感器•

传感器信息GUI（SensorInformation）机器人的传感器数据；（rviz插件接口）TF监听规划场景（可选）机器人状态发布其他接口•

外界环境信息（World

geometry

information）通过传感器建立的周围环境信息。规划场景模块的结构24©NXROBO20234.碰撞检测——可视化插件添加障碍添加障碍物的方式通过moveit_rviz插件通过程序接口手工添加通过传感器采集数据自动添加25©NXROBO20234.碰撞检测——避障运动规划$roslaunch

marm_moveit_configdemo.launch$rosrun

marm_planning

moveit_obstacles_demo.py自主避障规划例程注意：1)到网上下载功能包robot_marm，放到工作空间的src目录下，并编译此包；2)需将所有的python文件的权限设置为“可执行”；3)若ros版本为noetic，还需要将python文件的第一行的python改为python3。26©NXROBO20234.碰撞检测——避障运动规划marm_planning/scripts/moveit_obstacles_demo.py27©NXROBO20234.碰撞检测——避障运动规划marm_planning/scripts/moveit_obstacles_demo.py28©NXROBO20234.碰撞检测——Pick&Place实例演示与分析$roslaunch

marm_moveit_configdemo.launchPick&Place例程$rosrun

marm_planning

moveit_pick_and_place_demo.py注意：1)到网上下载功能包robot_marm，放到工作空间的src目录下，并编译此包；2)需将所有的python文件的权限设置为“可执行”；3)若ros版本为noetic，还需要将python文件的第一行的python改为python3。29©NXROBO20234.碰撞检测——Pick&Place实例演示与分析创建抓取的目标物体设置目标物体的放置位置30©NXROBO20234.碰撞检测——Pick&Place实例演示与分析生成抓取姿态Pick31©NXROBO20234.碰撞检测——Pick&Place实例演示与分析Place32©NXROBO20235.运动学插件的配置——KDLMoveIt!默认使用的运动学求解器，数值求解

优点：可求解封闭情况下逆运动学

缺点：速度慢、可能找不到解33©NXROBO20235.运动学插件的配置——TRAC-IK*参考链接：/kinetic/api/moveit_tutorials/html/doc/trac_ik/trac_ik_tutorial.html34©NXROBO20235.运动学插件的配置——MoveIt!TRAC-IK配置方法安装配置$sudoaptinstallros-noetic-trac-ik-kinematics-plugin$rosed

"$MYROBOT_NAME"_moveit_config/config/kinematics.yaml测试$sudo

marm_moveit_configdemo.launch*参考链接：/kinetic/api/moveit_tutorials/html/doc/trac_ik/trac_ik_tutorial.html35©NXROBO20235.运动学插件的配置——IKFast

IKFast，由Rosen

Diankov编写的OpenRAVE运动规划软件提供；

可以求解任意复杂运动链的运动学方程（解析解），并产生特定语言的文件（如C++）后供使用；

比较稳定、速度快，在最新的处理器上能以5微秒的速度完成运算。36©NXROBO20235.运动学插件的配置——MoveIt!IKFast配置方法$

gitclone/crigroup/openrave-installation$cdopenrave-installation$./install-dependencies.sh$./install-osg.sh$./install-fcl.sh$./install-openrave.sh安装openrave-installation$

sudoaptinstallcmakeg++gitipython3minizippython3-devpython3-h5pypython3-numpypython3-scipy

$sudoaptinstalllibassimp-devlibavcodec-devlibavformat-devlibboost-all-devlibboost-date-time-devlibbullet-devlibfaac-devlibglew-devlibgsm1-devliblapack-devliblog4cxx-devlibmpfr-devlibode-devlibogg-devlibpcrecpp0v5libpcre3-devlibqhull-devlibsoqt520libsoqt520-devlibswscale-devlibvorbis-devlibx264-devlibxml2-devlibxvidcore-dev安装程序与依赖库37©NXROBO20235.运动学插件的配置——MoveIt!IKFast配置方法$gitclone/openscenegraph/OpenSceneGraph.git$cdOpenSceneGraph$gitreset--hard{OSG_COMMIT}$mkdirbuild$cdbuild$cmake-DDESIRED_QT_VERSION=4..$sudomakeinstall_ld_conf$make-j`nproc`$sudomakeinstall安装OpenSceneGraph-3.4$gitclone/flexible-collision-library/fcl$cdfcl$gitreset--hard0.5.0$mkdirbuild$cdbuild$cmake..$make-j`nproc`$sudomakeinstall安装FlexibleCollisionLibrary0.5.038©NXROBO20235.运动学插件的配置——MoveIt!IKFast配置方法$sudoaptinstallpython3-pip$sudopip3installsympy$sudoaptremovepython3-mpmath安装Python工具安装IKFast和OpenRave功能包$gitclone-bproduction/rdiankov/openrave.git$cdopenrave$gitreset--hardRAVE_COMMIT(RAVE_COMMIT:cmake-DODE_USE_MULTITHREAD=ON-DOSG_DIR=/usr/local/lib64/\-DUSE_PYBIND11_PYTHON_BINDINGS:BOOL=TRUE-DBoost_NO_BOOST_CMAKE=1..)$mkdirbuild$cdbuild创建collada文件$exportMYROBOT_NAME="marm"$rosrunxacroxacro--inorder-o"$MYROBOT_NAME".urdf"$MYROBOT_NAME".xacro$rosruncollada_urdfurdf_to_collada"$MYROBOT_NAME".urdf"$MYROBOT_NAME".dae$exportIKFAST_PRECISION="5"$cp"$MYROBOT_NAME".dae"$MYROBOT_NAME".backup.dae$rosrunmoveit_kinematicsround_collada_numbers.py"$MYROBOT_NAME".dae"$MYROBOT_NAME".dae"$IKFAST_PRECISION"创建dae文件*参考链接：/kinetic/api/moveit_tutorials/html/doc/ikfast/ikfast_tutorial.html39©NXROBO20235.运动学插件的配置——MoveIt!IKFast配置方法$openrave-robot.py

"$MYROBOT_NAME".dae--infolinks查看生成的模型$openrave

"$MYROBOT_NAME".dae*参考链接：/kinetic/api/moveit_tutorials/html/doc/ikfast/ikfast_tutorial.html40©NXROBO20235.运动学插件的配置——MoveIt!IKFast配置方法$

export

PLANNING_GROUP="arm"$

export

BASE_LINK="1"生成程序文件$

export

EEF_LINK="11"$

exportIKFAST_OUTPUT_PATH=`pwd`/ikfast61_"$PLANNING_GROUP".cpp$

python`openrave-