机械臂控制C语言程序,ROS机械臂开发：MoveIt!编程
一、编程接口
提供C++、Python、GUI、命令行等接口
move_group节点
Python和C++接口示例
MoveIt关注更多的是plan工作，后面的execute需要控制器执行，MoveIt只是监控功能。
编程步骤
连接控制需要的规划组
左臂右臂
设置目标位姿
关节空间、笛卡尔空间
设置运动约束
可选，比如末端的水不能撒出来
使用MoveIt!规划一条到达目标的轨迹
修改轨迹
如果对plan不满意可以修改，比如速度等参数
二、关节空间运动
每个关节独立规划，不关注末端的笛卡尔轨迹
import rospy, sys
import moveit_commander
class MoveItFkDemo:
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_fk_demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = moveit_commander.MoveGroupCommander('manipulator')
# 设置机械臂运动的允许误差值
arm.set_goal_joint_tolerance(0.001)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_max_velocity_scaling_factor(0.5)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 设置机械臂的目标位置，使用六轴的位置数据进行描述(单位：弧度)
joint_positions = [0.391410, -0.676384, -0.376217, 0.0, 1.052834, 0.454125]
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
arm.go()
rospy.sleep(1)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
if __name__ == "__main__":
try:
MoveItFkDemo()
except rospy.ROSInterruptException:
pass
go()相当与plan再execute，阻塞函数
运行脚本后的效果：
代码运行效果
三、自主避障运动
MoveIt里有一个规划场景监听器，用于检测场景中是否有障碍物，障碍物有3种方式可以告诉监听器，1、通过rviz界面添加；2、通过程序(C++，python)添加；3、通过外部传感器，如kinect添加障碍物。这里先介绍前两种，通过外部传感器的方式在视觉章节里介绍。
规划场景的模块结构
通过rviz界面添加
启动demo.launch文件，点开scene objects标签，点Import File，可以选择solidworks建好的模型或者模型库中已有的模型添加进来，放到机器人可能的运动路径上，点击publish scene发布物体，告诉moveit场景中存在该物体。
Import File
image.png
设置目标位置，在拖动过程中，可以发现与障碍物接触的连杆变为红色高亮：
image.png
拖动到障碍物下方，plan并执行，可以看出机器人进行了自主避障，走出了“夸张”的轨迹。
避障运动
通过代码添加
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy, sys
import moveit_commander
from moveit_commander import RobotCommander, MoveGroupCommander, PlanningSceneInterface
from geometry_msgs.msg import PoseStamped
class MoveItWithObstacle:
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('motion_with_obstacle')
# 初始化场景对象
scene = PlanningSceneInterface()
rospy.sleep(1)
self.ur5 = MoveGroupCommander('manipulator')
self.robot = RobotCommander()
self.eef_link = self.ur5.get_end_effector_link()
self.ur5.set_goal_position_tolerance(0.01)
self.ur5.set_goal_orientation_tolerance(0.05)
self.ur5.allow_replanning(True)
self.ur5.set_planning_time(10)
self.ur5.set_named_target('home')
self.ur5.go()
# 移除场景中之前运行残留的物体
scene.remove_attached_object(self.eef_link, 'tool')
scene.remove_world_object('table')
scene.remove_world_object('tool')
rospy.sleep(2)
# 设置桌面的高度
table_ground = 0.9
# 设置table和tool的三维尺寸
table_size = [0.1, 0.7, 0.01]
tool_size = [0.02, 0.2, 0.02]
# 设置tool的位姿
tool_pose = PoseStamped()
# 相对于机器人终端描述位姿
tool_pose.header.frame_id = self.eef_link
tool_pose.pose.position.x = 0
tool_pose.pose.position.y = 0.08
tool_pose.pose.position.z = 0
tool_pose.pose.orientation.w = 0
box_name = 'tool'
scene.add_box(box_name, tool_pose, tool_size)
# 将tool附着到机器人的终端
touch_links = self.robot.get_link_names('manipulator')
scene.attach_box(self.eef_link, box_name, touch_links=touch_links)
rospy.sleep(2)
# 将table加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = 'base_link'
table_pose.pose.position.x = 0.25
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box('table', table_pose, table_size)
rospy.sleep(2)
def move(self):
# 更新当前的位姿
self.ur5.set_start_state_to_current_state()
# 设置机械臂的目标位置
target_pose = PoseStamped()
target_pose.header.frame_id = 'base_link'
target_pose.pose.position.x = 0.32577
target_pose.pose.position.y = 0.1176
target_pose.pose.position.z = 0.48681
target_pose.pose.orientation.x = 0.7071
target_pose.pose.orientation.y = 0
target_pose.pose.orientation.z = 0
target_pose.pose.orientation.x = 0.7071
self.ur5.set_pose_target(target_pose, self.eef_link)
# 控制机械臂完成运动
self.ur5.go()
rospy.sleep(1)
# 控制机械臂回到初始化位置
self.ur5.set_named_target('home')
self.ur5.go()
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
if __name__ == '__main__':
try:
move_with_obstacle = MoveItWithObstacle()
move_with_obstacle.move()
except rospy.ROSInterruptException:
pass
实现的效果是机器人末端带着工具避开障碍物：
代码添加障碍物
