编程语言
首页 > 编程语言> > 实际的机械臂控制(8)使用find_object3D和Kinect2实现目标跟踪(基于python)

实际的机械臂控制(8)使用find_object3D和Kinect2实现目标跟踪(基于python)

作者:互联网

话不多说了

在很多大佬的博客,主要是古月居的一些博客中,他们都介绍了使用find_object2D这个包是识别目标的位姿。但是如何将目标的位置和姿态发送给机械臂,他们都没有提及。这让我很尴尬呀,没人带入门,很生气,所以停止研究机械臂的控制,然后去继续告视觉部分,一不小心发了个定刊T-PAMI。
可能是借助于这个T-PAMI提供的元气,瞬间打通了我的任督二脉,让我瞬间-----------------------------------------------白瞟到了一个新的方法。

首先是安装find_object2D这个包,建议源码安装
具体安装细节参考这个教程

关于如何将目标的位置发送给机械臂,目前大佬们不知道是藏私,还是不屑于讲,在他们的博客里少有提及。
目前说的最明确的是这个博客
但是也只是说到了
将识别的坐标点发送给机械臂
在使用find_object_3d时,我们可以直接获得目标物体的tf坐标,因此可以使用ros自带的tf转换直接查询机械臂基座标到物体的tf关系,并发送给机械臂: 参考这里:http://docs.ros.org/lunar/api/tf/html/c++/classtf_1_1TransformListener.html http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28C%2B%2B%29

tf::StampedTransform transform;
     try{
       listener.lookupTransform("/arm_base_link", "/object_1",  
                                ros::Time(0), transform);
     }
     catch (tf::TransformException ex){
       ROS_ERROR("%s",ex.what());
       ros::Duration(1.0).sleep();
     }

看到这便我也是糊里糊涂,感觉懂了,但是感觉有不懂,因为我还是无法让机械臂动起来。
下面开始我白票的方法。

首先是发送位姿

1. 发送坐标转换矩阵

#!/usr/bin/env python

import rospy
import tf
from geometry_msgs.msg import Pose
def object_position_pose(t,o):
    pub = rospy.Publisher('/objection_position_pose',Pose,queue_size=10)
    p = Pose()
    rate = rospy.Rate(5)
    p.position.x = t[0]
    p.position.y = t[1]
    p.position.z = t[2]

    p.orientation.x = o[0]
    p.orientation.y = o[1]
    p.orientation.z = o[2]
    p.orientation.w = o[3]
    pub.publish(p)
    rate.sleep()

if __name__ == '__main__':
   rospy.init_node('tf_listener',anonymous=True)
   listener = tf.TransformListener() 
   rate = rospy.Rate(10.0)
   while not rospy.is_shutdown():
       try:
           (trans,rot) = listener.lookupTransform('/world', '/object_33', rospy.Time(0))
           print("trans:")
           print(trans)
           print("rot:")
           print(rot)
           object_position_pose(trans,rot)
       except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
           continue
       rate.sleep()

然后是控制机械臂的运动

2. 控制机械臂运动

#! /usr/bin/env python
import sys
import copy
import rospy
import moveit_commander
import geometry_msgs
import tf

from moveit_commander import MoveGroupCommander

from moveit_python import (MoveGroupInterface,
                           PlanningSceneInterface,
                           PickPlaceInterface)
import moveit_msgs.msg
from geometry_msgs.msg import Pose
from copy import deepcopy

def callback(pose):
    object_position_info = pose.position
    object_orientation_info = pose.orientation
    print object_position_info

    moveit_commander.roscpp_initialize(sys.argv)
    #rospy.init_node('move_group_grasp', anonymous=True)
    #robot = moveit_commander.robot.RobotCommander()

    arm_group = moveit_commander.move_group.MoveGroupCommander("manipulator")
    hand_group = moveit_commander.move_group.MoveGroupCommander("gripper") 
   
    arm_group.set_named_target("home_j")
    plan = arm_group.go()

    print("Point 1")

    # Open
    #hand_group.set_joint_value_target([9.800441184282249e-05, -9.800441184282249e-05,   9.800441184282249e-05, 9.800441184282249e-05, -9.800441184282249e-05, 9.800441184282249e-05])
    #hand_group.go(wait=True)
    #print("Point 2")
    hand_group.set_named_target("open")
    plan = hand_group.go()
    print("Point 2")

    pose_target = arm_group.get_current_pose().pose

    # Block point top
    pose_target.position.x = object_position_info.x
    pose_target.position.y = object_position_info.y
    pose_target.position.z = object_position_info.z+0.25



    arm_group.set_pose_target(pose_target)
    arm_group.go(wait=True)
    print("Point 3")

    # Block point1
    pose_target.position.z = pose_target.position.z-0.07



    arm_group.set_pose_target(pose_target)
    arm_group.go(wait=True)
    print("Point 4")
    rospy.sleep(3)

    hand_group.set_named_target("close")
    plan = hand_group.go()
    print("Point 5")
    rospy.sleep(2)

    pose_target.position.z = pose_target.position.z+0.25
    arm_group.set_pose_target(pose_target)
    plan = arm_group.go()
    print("Point 6")


    pose_target.position.x = pose_target.position.x + 0.5
    arm_group.go()
    rospy.sleep(3)
    print("Point 7")



    hand_group.set_named_target("open")
    plan = hand_group.go()
    print("Point 8")
        
        
    moveit_commander.roscpp_shutdown()

def object_position_sub():
    rospy.Subscriber("/objection_position_pose",Pose,callback,queue_size=10)
if __name__ == "__main__":
    rospy.init_node('object_position_sub_And_grasp_node',anonymous=True)
    object_position_sub()
    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            print("11")
            rospy.spin()
        except KeyboardInterrupt:
            print("Shutting down")
        rate.sleep()

需要提出的是,通过这种方法,机械臂规划的路径是很奇葩的,即使是两个很近的点,机械臂都要绕来绕去,绕一大圈。这很麻烦。
怎么解决呢?
方法是有的,那就是在笛卡尔空间规划机械臂的运动。
具体方法参考这个链接

3. 在笛卡尔空间规划末端路径

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)
 
        # 是否需要使用笛卡尔空间的运动规划,获取参数,如果没有设置,则默认为True,即走直线
        cartesian = rospy.get_param('~cartesian', True)
                      
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = MoveGroupCommander('manipulator')
        
        # 当运动规划失败后,允许重新规划
        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 = []
 
        # 如果为True,将初始位姿加入路点列表
        if cartesian:
            waypoints.append(start_pose)
            
        # 设置路点数据,并加入路点列表,所有的点都加入
        wpose = deepcopy(start_pose)#拷贝对象
        wpose.position.z -= 0.2
 
        if cartesian:  #如果设置为True,那么走直线
            waypoints.append(deepcopy(wpose))
        else:          #否则就走曲线
            arm.set_pose_target(wpose)  #自由曲线
            arm.go()
            rospy.sleep(1)
 
        wpose.position.x += 0.15
 
        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
        
        wpose.position.y += 0.1
 
        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
 
        wpose.position.x -= 0.15
        wpose.position.y -= 0.1
 
        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
 
 
        #规划过程
 
        if cartesian:
		fraction = 0.0   #路径规划覆盖率
		maxtries = 100   #最大尝试规划次数
		attempts = 0     #已经尝试规划次数
		
		# 设置机器臂当前的状态作为运动初始状态
		arm.set_start_state_to_current_state()
	 
		# 尝试规划一条笛卡尔空间下的路径,依次通过所有路点
		while fraction < 1.0 and attempts < maxtries:
        #规划路径 ,fraction返回1代表规划成功
		    (plan, fraction) = arm.compute_cartesian_path (
		                            waypoints,   # waypoint poses,路点列表,这里是5个点
		                            0.01,        # eef_step,终端步进值,每隔0.01m计算一次逆解判断能否可达
		                            0.0,         # jump_threshold,跳跃阈值,设置为0代表不允许跳跃
		                            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

到这里,机械臂的运动已经可以控制的很清晰了。
但是,有时候,我们希望控制的末端在6个自由度的其中一个自由度的运动。
这个时候我们需要这么做。

4. 单独控制某一个自由度的运动

首先看代码,主要参考这个网页

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

import rospy, sys
import moveit_commander
from moveit_msgs.msg import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint

from geometry_msgs.msg import PoseStamped, Pose
from tf.transformations import euler_from_quaternion, quaternion_from_euler

class MoveItIkDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)
        
        # 初始化ROS节点
        rospy.init_node('moveit_ik_demo')
                
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = moveit_commander.MoveGroupCommander('arm')
                
        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()
                        
        # 设置目标位置所使用的参考坐标系
        reference_frame = 'base_link'
        arm.set_pose_reference_frame(reference_frame)
                
        # 当运动规划失败后,允许重新规划
        arm.allow_replanning(True)
        
        # 设置位置(单位:米)和姿态(单位:弧度)的允许误差
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)
        
        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(2)
               
        # 设置机械臂工作空间中的目标位姿,位置使用x、y、z坐标描述,
        # 姿态使用四元数描述,基于base_link坐标系
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.header.stamp = rospy.Time.now()     
        target_pose.pose.position.x = 0.2593
        target_pose.pose.position.y = 0.0636
        target_pose.pose.position.z = 0.1787
        target_pose.pose.orientation.x = 0.70692
        target_pose.pose.orientation.y = 0.0
        target_pose.pose.orientation.z = 0.0
        target_pose.pose.orientation.w = 0.70729
        
        # 设置机器臂当前的状态作为运动初始状态
        arm.set_start_state_to_current_state()
        
        # 设置机械臂终端运动的目标位姿
        arm.set_pose_target(target_pose, end_effector_link)
        
        # 规划运动路径
        traj = arm.plan()
        
        # 按照规划的运动路径控制机械臂运动
        arm.execute(traj)
        rospy.sleep(1)
         
        # 控制机械臂终端向右移动5cm
        arm.shift_pose_target(1, -0.05, end_effector_link)
        arm.go()
        rospy.sleep(1)
  
        # 控制机械臂终端反向旋转90度
        arm.shift_pose_target(3, -1.57, end_effector_link)
        arm.go()
        rospy.sleep(1)
           
        # 控制机械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    MoveItIkDemo()

然后是代码解析。这边一定要看,

重要API整理

第一步初始化:与前面正运动初始化一样。

end_effector_link = arm.get_end_effector_link()

第二步:获取机械臂的终端link

reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)

第三步设置参考系:逆运动的位姿需要在笛卡尔坐标下描述,因此将base_link设置为指定的参考坐标系。

arm.set_named_target('home')
arm.go()
rospy.sleep(2)
arm.set_start_state_to_current_state()

第四步设置起始位姿:先将机械臂回到home位姿,然后设置该位姿为运动的起始位姿。

target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()     
target_pose.pose.position.x = 0.2593
target_pose.pose.position.y = 0.0636
target_pose.pose.position.z = 0.1787
target_pose.pose.orientation.x = 0.70692
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 0.70729
arm.set_pose_target(target_pose, end_effector_link)

第五步设置运动终点位姿:x,y,z描述end_effector_link在base_link坐标系下的空间位置;x,y,z,w四元数描述end_effector_link在base_link坐标系下的空间姿态。

traj = arm.plan()
arm.execute(traj)

第六步规划与执行:arm.plan()规划一条起始位姿到终点位姿的路径traj, arm.execute(traj)执行该路径。

arm.shift_pose_target(1, -0.05, end_effector_link)
arm.go()
rospy.sleep(1)
arm.shift_pose_target(3, -1.57, end_effector_link)
arm.go()
rospy.sleep(1)

除了使用PoseStamped描述位姿并规划外,还可以使用shift_pose_target实现单轴方向上的目标设置与规划;
第一个参数:描述机器人在六个自由度中实现哪一种运动,0,1,2,3,4,5分别表示xyz三个方向的平移与旋转。
第二个参数:描述机器人移动或旋转的量,单位为m或者弧度。
第三个参数:描述该运动针对的对象。

标签:set,target,python,pose,object3D,rospy,Kinect2,position,arm
来源: https://blog.csdn.net/qq_33328642/article/details/122667192