简介
本文使用 wheeltec，代码中型号为mini_mec_moveit_six，6自由度机械臂小车。实现用深度相机，检测目标的3D位姿，从图像坐标系转换到机器人坐标系，并用机械臂moveit抓取目标。完整流程的大概实现。
开启机械臂底层节点，包含了moveit
roslaunch wheeltec_arm_pick base_serial.launch
moveit 包含在内
<!-- 开启虚拟机械臂 -->
<include if="$(arg moveit_config)" file='/home/wheeltec/wheeltec_arm/src/$(arg car_mode)_moveit_config/launch/demo.launch'>
<arg if="$(eval voi_arm_1==true and moveit_num=='six_arm')" name="voi_arm" value="false"/>
</include>
开启深度相机
小车自带的是astra深度相机
roslaunch astra_camera astra.launch
会有相机的TF坐标无法转换的问题，在虚拟机打开rviz，配置Global Options，Fixed Frame修改为base_link，rviz中TF status Warn警告
No transform from [camera_link] to frame [base_link]
则需要配置相机的TF坐标系
配置相机的TF坐标系
修改robot_model_visualization.launch默认设备mini_mec_moveit_six
<arg name="car_mode" default="mini_mec_moveit_six" />
运行
roslaunch turn_on_wheeltec_robot robot_model_visualization.launch
主要用到了tf的static_transform_publisher，发布静态坐标变换，只能发布两个静止的坐标系间的坐标变换
<group if="$(eval car_mode == 'mini_mec_moveit_six')">
<!-- 用于雷达节点，后面同理不再赘述-->
<!--node pkg="tf" type="static_transform_publisher" name="base_to_laser" args="0.048 0.00 0.18 3.925 0 0 base_footprint laser 100" /--> <!--A2 laser-->
<node pkg="tf" type="static_transform_publisher" name="base_to_laser" args="0.048 0.00 0.11 3.14 0 0 base_footprint laser 100" /> <!--A1 laser-->
<!-- 用于摄像头相关节点，如3d建图导航，后面同理不再赘述-->
<node pkg="tf" type="static_transform_publisher" name="base_to_camera" args="-0.118 0 0.55 0 0 0 base_footprint camera_link 100" />
<node pkg="tf" type="static_transform_publisher" name="base_to_gyro" args="0 0 0 0 0 0 base_footprint gyro_link 100" />
</group>
find-object-2d
我们希望机械臂抓住想要的任何东西，比通过贴标签和纯色物体更加智能一点，可以用find-object-2d模板匹配算法识别物体
安装
sudo apt-get install ros-$ROS_DISTRO-find-object-2d
运行
roslaunch find_object_2d find_object_3d.launch
find_object_2d所需的下面3个topic
Find-Object subscribed to :
/camera/rgb/image_rect_color
/camera/depth_registered/image_raw
/camera/rgb/camera_info
小车自带的astra深度相机启动时就默认发布的topic包含上面3个
launch源码大概如下，可以就用默认的，不用新增launch
<launch>
<!-- Example finding 3D poses of the objects detected -->
<!-- Which image resolution: sd, qhd, hd -->
<arg name="resolution" default="qhd" />
<node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen">
<param name="gui" value="true" type="bool"/>
<param name="settings_path" value="~/.ros/find_object_2d.ini" type="str"/>
<param name="subscribe_depth" value="true" type="bool"/>
<param name="objects_path" value="" type="str"/>
<param name="object_prefix" value="object" type="str"/>
<remap from="rgb/image_rect_color" to="camera/rgb/image_rect_color"/>
<remap from="depth_registered/image_raw" to="camera/depth_registered/image_raw"/>
<remap from="depth_registered/camera_info" to="camera/rgb/camera_info"/>
</node>
</launch>
效果
find_object_2d 的界面
在rviz中可以看到识别物体的位姿
然后需要把目标object_4的位姿发给机械臂
图像坐标系转换机器人坐标系
lookup_transform.py 代码如下
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import tf
from geometry_msgs.msg import Pose
def object_position_pose(t,o):
#话题名为： objection_position_pose，
#后面订阅这个话题时要使用这个名字
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)
rospy.sleep(2)
while not rospy.is_shutdown():
#主要需要修改的就是这个地方，前面的base_link是你的机械臂的世界坐标系，一般不要修改，后面的object_4是目标坐标系的名字，需要修改，这个名字需要与你在rviz下面看到的名字保持一致.看下图。
#这一步就是将目标的坐标从目标坐标系转换到世界诶坐标系。注意这个(trans,rot) ，后面会用到，分别表示旋转和平移。
try:
(trans,rot) = listener.lookupTransform('/base_link', '/object_4', rospy.Time(0))
print("trans:")
print(trans)
print("rot:")
print(rot)
object_position_pose(trans,rot)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print("exception")
#break
continue
rate.sleep()
其中一句
rospy.sleep(2)
很重要，如果没有，会报错
tf2.LookupException: "base_link" passed to lookupTransform argument target_frame does not exist.
然后
rosrun moveit_py lookup_transform.py
moveit_py 修改为你的workspace，src下包的目录名
控制机械臂
小车底层代码使用串口通信，给机械臂的传6个弧度值，来控制机械臂运动，不能控制机械臂末端运动到指定3D空间坐标，但是它支持了moveit。可以通过moveit传3D坐标，位姿，去控制。
moveit 使用代码如下
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy, sys
import moveit_commander
from geometry_msgs.msg import PoseStamped, Pose
import moveit_msgs.msg
import geometry_msgs.msg
from copy import deepcopy
class MoveItDemo:
def __init__(self):
print("============ Starting MoveItDemo")
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
print("============ Starting MoveItDemo 2")
# 初始化ROS节点
rospy.init_node('moveit_py_demo', anonymous=True)
print("============ Starting MoveItDemo 3")
# 初始化需要使用move group控制的机械臂中的arm group
#arm = moveit_commander.MoveGroupCommander('arm')
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
arm = moveit_commander.MoveGroupCommander("arm")
print("============ Starting MoveItDemo 4")
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
print("============ end_effector_link:",end_effector_link)
# 用于调式，打印机器人的状态
print("============ Printing robot state")
print(robot.get_current_state())
print("============")
#print("============ getJointNames",robot.getJointNames())
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
# arm.set_goal_position_tolerance(0.001)
# arm.set_goal_orientation_tolerance(0.01)
arm.set_goal_position_tolerance(0.1)
arm.set_goal_orientation_tolerance(0.1)
joint_goal = arm.get_current_joint_values()
print("============ joint_goal:",joint_goal)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_max_velocity_scaling_factor(0.5)
# 控制机械臂先回到初始化位置
print("============ arm_home")
arm.set_named_target('arm_home')
arm.go()
rospy.sleep(2)
# print("============ arm_uplift")
# arm.set_named_target('arm_uplift')
# arm.go()
# rospy.sleep(2)
# print("============ arm_clamp")
# arm.set_named_target('arm_clamp')
# arm.go()
# rospy.sleep(2)
current_pose = arm.get_current_pose(end_effector_link).pose
print("============ current_pose:", current_pose)
print("============ target_pose")
# 设置机械臂工作空间中的目标位姿，位置使用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.1
# target_pose.pose.position.y = 0.05
# target_pose.pose.position.z = 0.1
# 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)
# pose_target = geometry_msgs.msg.Pose()
# pose_target.position.x = current_pose.position.x
# pose_target.position.y = current_pose.position.y
# pose_target.position.z = current_pose.position.z -0.1
# arm.set_pose_target(pose_target)
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
waypoints.append(current_pose)
# 设置路点数据，并加入路点列表
wpose = deepcopy(current_pose)
wpose.position.z -= 0.04
waypoints.append(deepcopy(wpose))
wpose.position.x += 0.05
waypoints.append(deepcopy(wpose))
wpose.position.y += 0.01
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(10)
# 控制机械臂回到初始化位置
print("============ arm_home")
arm.set_named_target('arm_home')
arm.go()
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def test2():
print("============ Starting tutorial setup")
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("arm")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory)
print("============ Waiting for RVIZ...")
#rospy.sleep(5)
print("============ Starting tutorial")
# 打印参考系的名称
print("============ planning_frame: %s" % group.get_planning_frame())
# 打印这个组的末端执行器的连接名称
print("============ end_effector_link: %s" % group.get_end_effector_link())
# 获得机器人的所有组
print("============ Robot Groups:" , robot.get_group_names())
# 用于调式，打印机器人的状态
print("============ Printing robot state")
print(robot.get_current_state())
print("============")
# 为这个组规划动作，实现末端执行器到达期望的姿态目标。
print("============ Generating plan 1")
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 1.0
pose_target.position.x = 0.02
pose_target.position.y = -0.02
pose_target.position.z = 0.01
group.set_pose_target(pose_target)
# 调用规划器计算规划并在Rviz里显示，注意：这里只是规划，不是实际执行。
plan1 = group.plan()
print("============ Waiting while RVIZ displays plan1...")
rospy.sleep(5)
# 你可以要求Rviz显示规划（或叫轨迹），但group.plan()方法会自动实现。
print("============ Visualizing plan1")
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan1)
display_trajectory_publisher.publish(display_trajectory)
# print("============ Waiting while plan1 is visualized (again)...")
# rospy.sleep(5)
# # 移动到姿态目标, 因为它是一个阻塞函数，需要一个控制器是激活，执行后报告成功的轨迹
# group.go(wait=True)
# # 规划到连接空间目标
# group.clear_pose_targets()
# # 取得组目前设置的连接值
# group_variable_values = group.get_current_joint_values()
# print("============ Joint values: ", group_variable_values)
# # 修改其中一个关节，规划移动到新的连接空间目标，可视化规划。
# group_variable_values[0] = 1.0
# group.set_joint_value_target(group_variable_values)
# plan2 = group.plan()
# print("============ Waiting while RVIZ displays plan2...")
# rospy.sleep(5)
def inverse_kinematics():
print("逆解")
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("arm")
end_effector_link = group.get_end_effector_link()
#123
group.set_pose_reference_frame('base_link')
group.allow_replanning(True)
print("============ arm_home")
group.set_named_target('arm_home')
group.go()
rospy.sleep(2)
target_pose = group.get_current_pose(end_effector_link).pose
print("============ current_pose:", target_pose)
target_pose.position.x = target_pose.position.x+0.08;
target_pose.position.y = target_pose.position.y-0.05;
target_pose.position.z = target_pose.position.z-0.05;
group.set_start_state_to_current_state()
group.set_pose_target(target_pose, end_effector_link)
#group.set_plan_time(20.0)
plan = group.plan()
group.execute(plan)
target_pose = group.get_current_pose(end_effector_link).pose
print("============ current_pose:", target_pose)
rospy.sleep(10)
# 控制机械臂回到初始化位置
print("============ arm_home")
group.set_named_target('arm_home')
group.go()
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
if __name__ == "__main__":
# MoveItDemo()
# test2()
inverse_kinematics()
有2种方法，一种是逆解，一种是笛卡尔路径
因为该小车的深度相机需要大于0.6m才能获得图像深度，但是机械臂没有0.6m长，实际只能简单模拟，通过位姿控制机械臂的运动
扫一扫
5023
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
