ROS moveit 机械臂笛卡尔空间运动

最新推荐文章于 2024-05-26 16:30:10 发布

tutu_321

最新推荐文章于 2024-05-26 16:30:10 发布

阅读量6.3k

点赞数 12

文章标签： ROS moveit python

本文链接：https://blog.csdn.net/sinat_38625360/article/details/103042802

版权

机械臂moveit编程（python）

机械臂在笛卡尔空间的运动只能走点到点的直线运动，通过将位姿点加入waypoints中，机械臂会一次按照waypoints中的唯一依次沿直线运动到下一个点。

程序流程：
1.初始化需要控制的规划组；
2.设置运动约束（可选）；
3.设置终端link；
4.设置坐标系；
5.建立一个waypoints空列表，将所需要到达的位姿加入waypoints列表；
6.用compute_catesian_path AP进行求解。

笛卡尔空间画折线

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander
from geometry_msgs.msg import Pose
from copy import deepcopy

class MoveItCartesianDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_cartesian_demo', anonymous=True)
      
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = MoveGroupCommander('arm')
        
        # 当运动规划失败后，允许重新规划
        arm.allow_replanning(True)
        
        # 设置目标位置所使用的参考坐标系
        arm.set_pose_reference_frame('base_link')
                
        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
        arm.set_goal_position_tolerance(0.001)
        arm.set_goal_orientation_tolerance(0.001)
        
        # 设置允许的最大速度和加速度
        arm.set_max_acceleration_scaling_factor(0.5)
        arm.set_max_velocity_scaling_factor(0.5)
        
        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()

        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)
                                               
        # 获取当前位姿数据最为机械臂运动的起始位姿
        start_pose = arm.get_current_pose(end_effector_link).pose
                
        # 初始化路点列表
        waypoints = []

        # 将初始位姿加入路点列表
     
        waypoints.append(start_pose)
            
        # 设置路点数据，并加入路点列表
        wpose = deepcopy(start_pose)
        wpose.position.z -= 0.2
        waypoints.append(deepcopy(wpose))
        

        wpose.position.x += 0.1
        waypoints.append(deepcopy(wpose))
       
        
        wpose.position.y += 0.075
        waypoints.append(deepcopy(wpose))

		wpose.position.y -= 0.075
        waypoints.append(deepcopy(wpose))

		wpose.position.x -= 0.1
        waypoints.append(deepcopy(wpose))

		wpose.position.x += 0.05
        wpose.position.y += 0.075
        waypoints.append(deepcopy(wpose))

		wpose.position.y -= 0.15
        waypoints.append(deepcopy(wpose))


		wpose.position.x -= 0.05
        wpose.position.y += 0.075
        waypoints.append(deepcopy(wpose))

        
        wpose.position.x -= 0.03
        wpose.position.y -= 0.075
        waypoints.append(deepcopy(wpose))
       
		fraction = 0.0   #路径规划覆盖率
		maxtries = 100   #最大尝试规划次数
		attempts = 0     #已经尝试规划次数
		
		# 设置机器臂当前的状态作为运动初始状态
		arm.set_start_state_to_current_state()
	 
		# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
		while fraction < 1.0 and attempts < maxtries:
	    	(plan, fraction) = arm.compute_cartesian_path (
		                    waypoints,   # waypoint poses，路点列表
		                    0.01,        # eef_step，终端步进值
		                    0.0,         # jump_threshold，跳跃阈值
		                    True)        # avoid_collisions，避障规划
		    
		    # 尝试次数累加
	    	attempts += 1
		    
		    # 打印运动规划进程
	   	 if attempts % 10 == 0:
		   	 rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
		             
			# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
	  	 if fraction == 1.0:
			rospy.loginfo("Path computed successfully. Moving the arm.")
			arm.execute(plan)
			rospy.loginfo("Path execution complete.")
		# 如果路径规划失败，则打印失败信息
	   	 else:
			rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  

			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:
        MoveItCartesianDemo()
    except rospy.ROSInterruptException:
        pass

重点API整理

第一步初始化：与上一篇初始化一样。

moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_cartesian_demo', anonymous=True)
arm = MoveGroupCommander('arm')

第二步：获取机械臂的终端link及设置坐标系。

arm.set_pose_reference_frame('base_link')
end_effector_link = arm.get_end_effector_link()

第三步：将机械臂回到home位姿，当前位姿数据最为机械臂运动的起始位姿，然后设置机器臂当前的状态作为运动初始状态。

arm.set_named_target('home')
arm.go()
rospy.sleep(1)
start_pose = arm.get_current_pose(end_effector_link).pose
arm.set_start_state_to_current_state()

第四步：建立waypoints空列表，先将起始位姿加入列表中，然后依次加入其他位姿，位姿可以自己根据需要随便定义，依次将所定义的位姿加入waypoints中，此时并没有开始规划。

 waypoints = []
 waypoints.append(start_pose)

 wpose = deepcopy(start_pose)
 wpose.position.z -= 0.2
 waypoints.append(deepcopy(wpose))     

 wpose.position.x += 0.1
 waypoints.append(deepcopy(wpose))    
        
 wpose.position.y += 0.075
 waypoints.append(deepcopy(wpose))

 wpose.position.y -= 0.075
 waypoints.append(deepcopy(wpose))

 wpose.position.x -= 0.1
 waypoints.append(deepcopy(wpose))

 wpose.position.x += 0.05
 wpose.position.y += 0.075
 waypoints.append(deepcopy(wpose))

 wpose.position.y -= 0.15
 waypoints.append(deepcopy(wpose))

 wpose.position.x -= 0.05
 wpose.position.y += 0.075
 waypoints.append(deepcopy(wpose))
        
 wpose.position.x -= 0.03
 wpose.position.y -= 0.075
 waypoints.append(deepcopy(wpose))

第五步：这一步是整个笛卡尔运动规划的核心，使用笛卡尔路径规划API compute_cartesian_path（），其输入参数有四个：
第一个参数为之前创建的路径点列表；
第二个参数为终端步进值，即每一步规划的步长；
第三个参数为跳跃阈值，每一步规划时，有无法达到的点是否跳过；
第四个参数用于运动过程中是否考虑避障。
compute_cartesian_path（）执行后会有两个返回值：
plan表示规划出来的运动路径；
fraction表示规划路径的覆盖率，范围在0到1之间，fraction为1表示路径规划成功。

fraction = 0.0   #路径规划覆盖率
maxtries = 100   #最大尝试规划次数
attempts = 0     #已经尝试规划次数

while fraction < 1.0 and attempts < maxtries:
	    (plan, fraction) = arm.compute_cartesian_path (
		                    waypoints,   # waypoint poses，路点列表
		                    0.01,        # eef_step，终端步进值
		                    0.0,         # jump_threshold，跳跃阈值
		                    True)        # avoid_collisions，避障规划
		    
		    # 尝试次数累加
	    	attempts += 1
		    
		    # 打印运动规划进程
	   	 if attempts % 10 == 0:
		   	 rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
		             
			# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
	  	 if fraction == 1.0:
			rospy.loginfo("Path computed successfully. Moving the arm.")
			arm.execute(plan)
			rospy.loginfo("Path execution complete.")
		# 如果路径规划失败，则打印失败信息
	   	 else:
			rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")

该程序运行结果如下图，运行轨迹为在一个平面上写了一个王字。
在这里插入图片描述

笛卡尔空间画圆弧

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander
from geometry_msgs.msg import Pose, PoseStamped
from copy import deepcopy

import math
import numpy

class MoveItCartesianDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_cartesian_demo', anonymous=True)

      
                      
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = MoveGroupCommander('arm')
        
        # 当运动规划失败后，允许重新规划
        arm.allow_replanning(True)
        
        # 设置目标位置所使用的参考坐标系
		reference_frame = 'base_link'
        arm.set_pose_reference_frame('base_link')
                
        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
        arm.set_goal_position_tolerance(0.001)
        arm.set_goal_orientation_tolerance(0.001)
        
        # 设置允许的最大速度和加速度
        arm.set_max_acceleration_scaling_factor(0.5)
        arm.set_max_velocity_scaling_factor(0.5)
        
        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()

        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)
        
        # 设置路点数据，并加入路点列表
                                         
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.header.stamp = rospy.Time.now()     
        target_pose.pose.position.x = 0.331958
        target_pose.pose.position.y = 0.0
        target_pose.pose.position.z = 0.307887
        target_pose.pose.orientation.x = -0.482974
        target_pose.pose.orientation.y = 0.517043
        target_pose.pose.orientation.z = -0.504953
        target_pose.pose.orientation.w = -0.494393
        
        # 设置机械臂终端运动的目标位姿
        arm.set_pose_target(target_pose, end_effector_link)
        arm.go()

        # 初始化路点列表
        waypoints = []
                
        # 将圆弧上的路径点加入列表
        waypoints.append(target_pose.pose)

        centerA = target_pose.pose.position.y
        centerB = target_pose.pose.position.z
        radius = 0.1

        for th in numpy.arange(0, 6.28, 0.02):
            target_pose.pose.position.y = centerA + 0.5*radius * math.cos(th)
            target_pose.pose.position.z = centerB + 1.2*radius * math.sin(th)
            wpose = deepcopy(target_pose.pose)
            waypoints.append(deepcopy(wpose))

       
	fraction = 0.0   #路径规划覆盖率
	maxtries = 100   #最大尝试规划次数
	attempts = 0     #已经尝试规划次数
		
	# 设置机器臂当前的状态作为运动初始状态
	arm.set_start_state_to_current_state()
	 
	# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
	while fraction < 1.0 and attempts < maxtries:
	    (plan, fraction) = arm.compute_cartesian_path (
		                    waypoints,   # waypoint poses，路点列表
		                    0.01,        # eef_step，终端步进值
		                    0.0,         # jump_threshold，跳跃阈值
		                    True)        # avoid_collisions，避障规划
		    
		    # 尝试次数累加
	    attempts += 1
		    
		    # 打印运动规划进程
	    if attempts % 10 == 0:
		    rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
		             
		# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
	    if fraction == 1.0:
		rospy.loginfo("Path computed successfully. Moving the arm.")
		arm.execute(plan)
		rospy.loginfo("Path execution complete.")
		# 如果路径规划失败，则打印失败信息
	    else:
		rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  

		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:
        MoveItCartesianDemo()
    except rospy.ROSInterruptException:
        pass

编程思想与上面画折线完全一致，唯一的区别在与waypoints列表，将圆表示为参数方程。

waypoints = []
 # 将圆弧上的路径点加入列表
waypoints.append(target_pose.pose)
centerA = target_pose.pose.position.y
centerB = target_pose.pose.position.z
radius = 0.1

for th in numpy.arange(0, 6.28, 0.02):
        target_pose.pose.position.y = centerA + radius * math.cos(th)
        target_pose.pose.position.z = centerB + radius * math.sin(th)
        wpose = deepcopy(target_pose.pose)
        waypoints.append(deepcopy(wpose))

运行结果如下：在这里插入图片描述
同理，只要知道曲线的参数方程，就可以在笛卡尔空间中画曲线，例如画椭圆。

tutu_321

关注

12
点赞
踩
97

收藏

觉得还不错? 一键收藏
5
评论
ROS moveit 机械臂笛卡尔空间运动

机械臂moveit编程（python）机械臂在笛卡尔空间的运动只能走点到点的直线运动，通过将位姿点加入waypoints中，机械臂会一次按照waypoints中的唯一依次沿直线运动到下一个点。程序流程：1.初始化需要控制的规划组；2.设置运动约束（可选）；3.设置终端link；4.设置坐标系；5.建立一个waypoints空列表，将所需要到达的位姿加入waypoints列表；6.用...
复制链接

扫一扫