ros ur5模拟扫描

任务描述：

项目是做个UR的仿真，结果要求UR5的末端在已知工作台上零件的构造以后可以距离零件固定的长度扫描零件，零件是不规则形状的（可以先按球来做），工作台可以旋转，要求在旋转的时候也可以扫描。

解决思路：

在网上找到ur5机械臂的urdf模型，并在此基础上添加可以旋转的正方体，用moveit配置运动学规划。在整个过程中，只扫描地面和四周，共五个面。其中四周四个面扫描过程是一致的，只需要在每次扫描时旋转正方体。规划扫描面上的三维坐标点：根据正方形的特点，从上往下从左往右，取一定的距离间隔，得出一连串的三维坐标点。将得到的三维坐标点进行笛卡尔空间运动规划，保证机械臂末端和平面是垂直的。同时，考虑到扫描棱边的情况，每当经过棱边时，将末端进行一定角度地偏转，使得整体上机械臂末端是沿着正方体表面进行扫描的。主要的难点是机械臂和正方体的相对位置确定，要保证扫描时在工作空间内，没有奇异点；扫描路径的规划，五个面的扫描顺序，以及中间的衔接过程；为了保持机械臂末端位姿是想要的效果，需要在笛卡尔空间进行运动规划，为了避免奇异值的出现，增大运动误差。

#!/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
import os# python 使用类创建结构体
class Myclass(object):class Struct(object):def __init__(self, x, y, z):self.x = xself.y = yself.z = zdef make_struct(self, x, y, z):return self.Struct(x, y, z)class MoveItCartesianDemo:def __init__(self):myclass=Myclass()point_a=myclass.make_struct(0.56,-0.04,0.1)point_b=myclass.make_struct(0.56,0.04,0.1)point_c=myclass.make_struct(0.56,0.04,0.02)point_d=myclass.make_struct(0.56,-0.04,0.02)err_len=0.09err_deg=0.05# 初始化move_group的APImoveit_commander.roscpp_initialize(sys.argv)# 初始化ROS节点rospy.init_node('moveit_cartesian_demo', anonymous=True)# 是否需要使用笛卡尔空间的运动规划cartesian = rospy.get_param('~cartesian', True)# 初始化需要使用move group控制的机械臂中的arm grouparm = MoveGroupCommander('arm')box = MoveGroupCommander('box')# 当运动规划失败后，允许重新规划arm.allow_replanning(True)# 设置目标位置所使用的参考坐标系arm.set_pose_reference_frame('base_link')# 设置位置(单位：米)和姿态（单位：弧度）的允许误差arm.set_goal_position_tolerance(0.01) #025arm.set_goal_orientation_tolerance(0.005)box.set_goal_joint_tolerance(0.001)end_effector_box = box.get_end_effector_link()box.set_joint_value_target([0])box.go()# 获取终端link的名称end_effector_link = arm.get_end_effector_link()# 控制机械臂回到初始化位置# print('go home ...')# arm.set_named_target('home')# arm.go()# os._exit(0)# 获取当前位姿数据最为机械臂运动的起始位a_pose = arm.get_current_pose(end_effector_link).posestart_pose = arm.get_current_pose(end_effector_link).pose#以a点为起始点a_pose.orientation.w=0.707a_pose.orientation.x= 0a_pose.orientation.y=0a_pose.orientation.z=-0.707print('go to point a ...')a_pose.position.x = point_a.x -err_lena_pose.position.y = point_a.ya_pose.position.z = point_a.zarm.set_pose_target(a_pose)arm.go()# os._exit(0)# 初始化路点列表waypoints = []wpose = deepcopy(start_pose)box_width=0.08diff=0.02n=int(box_width/diff)+1for i in range(n):wpose.orientation.w=0.707wpose.orientation.x= 0wpose.orientation.y=0wpose.orientation.z=-0.707wpose.position.z=point_a.z-i*diffwpose.position.x=point_a.x - err_lenif i%2==0:wpose.position.y=point_a.yelse:wpose.position.y=point_b.y# print('move {} point'.format(i+1))waypoints.append(deepcopy(wpose))if i%2==0:wpose.position.y=point_b.yelse:wpose.position.y=point_a.ywaypoints.append(deepcopy(wpose))if i%2==0:wpose.position.y+=0.035wpose.orientation.w=0.5wpose.orientation.x= 0wpose.orientation.y=0wpose.orientation.z=-0.866waypoints.append(deepcopy(wpose))else:wpose.position.y-=0.035wpose.orientation.w=0.866wpose.orientation.x= 0wpose.orientation.y=0wpose.orientation.z=-0.5waypoints.append(deepcopy(wpose))if cartesian: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.,         # jump_threshold，跳跃阈值False)        # avoid_collisions，避障规划# 尝试次数累加attempts += 1# 打印运动规划进程if attempts % 50 == 0:rospy.loginfo("Still trying after " + str(attempts) + " attempts...")# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动if fraction == 1.0:fraction=0attempts=0arm.execute(plan)#box旋转90度pi12=1.57box.set_start_state_to_current_state()print('start rotate......')for num in range(3):print('times  {}'.format(num))joint_value=(num+1)*pi12box.set_joint_value_target([joint_value])box.go()# box.set_start_state_to_current_state()rospy.sleep(0.1)curr_pose = arm.get_current_pose(end_effector_link).poseif num==0:way=[]way.append(deepcopy(curr_pose))way+=waypointsarm.set_start_state_to_current_state()while fraction < 1.0 and attempts < maxtries:(plan_again, fraction) = arm.compute_cartesian_path (way,   # waypoint poses，路点列表0.01,        # eef_step，终端步进值0.,         # jump_threshold，跳跃阈值False)        # avoid_collisions，避障规划attempts += 1              # 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动if fraction == 1.0:fraction=0rospy.loginfo("go  again ...")arm.execute(plan_again)# 如果路径规划失败，则打印失败信息else:rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  # 获取当前位姿数据最为机械臂运动的起始位start_pose = arm.get_current_pose(end_effector_link).posewaypoints=[]waypoints.append(deepcopy(start_pose))start_pose.position.x-=0.1start_pose.position.y+=0.1waypoints.append(deepcopy(start_pose))start_pose.position.z+=0.1waypoints.append(deepcopy(start_pose))start_pose=deepcopy(a_pose)#以a点为起始点start_pose.orientation.w=0.707start_pose.orientation.x=-0.707start_pose.orientation.y=0start_pose.orientation.z=0print('go to point a .....')start_pose.position.x = point_a.xstart_pose.position.y = point_a.ystart_pose.position.z = point_a.z+err_lenwaypoints.append(deepcopy(start_pose))wpose=deepcopy(start_pose)for i in range(n):if i==0:wpose.orientation.w=0.612wpose.orientation.x=-0.612wpose.orientation.y=-0.354wpose.orientation.z=0.354waypoints.append(deepcopy(wpose))elif i==n-1:wpose.orientation.w=0.612wpose.orientation.x=-0.612wpose.orientation.y=0.354wpose.orientation.z=-0.354waypoints.append(deepcopy(wpose))else:wpose.orientation.w=0.707wpose.orientation.x=-0.707wpose.orientation.y=0wpose.orientation.z=0wpose.position.x=point_a.x+i*diffif i%2==0:wpose.position.y=point_a.yelse:wpose.position.y=point_b.ywpose.position.z=point_a.z+err_len# print('move {} point'.format(i+1))waypoints.append(deepcopy(wpose))if i%2==0:wpose.position.y=point_b.yelse:wpose.position.y=point_a.ywaypoints.append(deepcopy(wpose))if i%2==0:wpose.position.y+=0.035wpose.orientation.w=0.5wpose.orientation.x= -0.866wpose.orientation.y= 0wpose.orientation.z= 0waypoints.append(deepcopy(wpose))else:wpose.position.y-=0.035wpose.orientation.w=0.866wpose.orientation.x= -0.5wpose.orientation.y= 0wpose.orientation.z= 0waypoints.append(deepcopy(wpose))# 设置机器臂当前的状态作为运动初始状态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.,         # jump_threshold，跳跃阈值False)        # avoid_collisions，避障规划if fraction == 1.0:fraction=0attempts=0arm.execute(plan)# 关闭并退出moveitmoveit_commander.roscpp_shutdown()moveit_commander.os._exit(0)if __name__ == "__main__":try:MoveItCartesianDemo()except rospy.ROSInterruptException:pass

ros ur5模拟扫描相关推荐

获取微信小程序码传递的参数 / 微信开发者工具模拟扫描小程序码调试
本文主要介绍如何在微信开发者工具中,模拟微信扫描小程序码打开小程序的场景,进行调试. 二维码调试可以看这篇文章:微信开发者工具模拟扫描二维码调试添加编译模式添加一个咱们自定义的编译模式输入模式名 ...
ROS可以不扫描地图，自己制作地图
以下实验在保福寺桥世纪科贸大厦B座亲测 1 找一个平面图如下 2 然后随便找个什么软件,我用的是MATLAB 处理一下把其中大于阈值的点找出来,形成文件如下 3 利用ROS发布地图,首先 ...
ros之模拟导航算法
导航算法在move_base节点中,当修改一些规划路线的参数时,又不想每次实际去跑,又想有定量的分析. 就可以用rosbag方式. 实现步骤: 1.记录log数据 rosbag recoved -O ...
利用Blensor模拟扫描生成点云
编写脚本实现多站点云导出到同一个文件中,其中需要对每站数据进行旋转平移变换,否则点云数据一团糟,没有使用价值.附乱七八糟的代码(防自己忘记 import bpy import math import ...
27.Silverlight二维旋转+平面渐变+动画，模拟雷达扫描图之基本框架
在现实生活中的雷达运行扫描到物体的某一些属性(比如:速度,频率)超过安全范围值将会报警.在实际的某一些项目中(比如监控系统),我们也想制作一个雷达扫描图来模拟扫描各种设备那么应该如何做呢? 我们将各种 ...
Silverlight实用窍门系列：28.Silverlight制作随机分布雷达扫描点，模拟雷达扫描图之被扫描设备【附带源码实例】...
实际项目中,我们模拟一个监控多台电脑的雷达扫描图效果.我们假设发现了很多台设备,这些设备具有CPU使用率这个属性,在雷达扫描的时候,如果CPU的值高于90则报警为红色. 本节实例建立在上一节的基础之上 ...
Silverlight实用窍门系列：27.Silverlight二维旋转+平面渐变+动画，模拟雷达扫描图之基本框架【附带源码实例】...
在现实生活中的雷达运行扫描到物体的某一些属性(比如:速度,频率)超过安全范围值将会报警.在实际的某一些项目中(比如监控系统),我们也想制作一个雷达扫描图来模拟扫描各种设备那么应该如何做呢? 我们将各种 ...
ROS机器人操作系统资料与资讯（2018年8月）
暑假好快,转眼开学,假期基本都在出差,工作这些年,发现ROS讨论最火的时候基本在2015-17年,进入18年,基本教程已经完全普及,课程体系也已经完备,国内基本机器人相关专业和方向,都已经逐步开展基于 ...
Spring扫描类过程解析和案例
文章目录 1. 前言 2. 源码分析 2.1 主要入口 2.2 scanCandidateComponents 2.3 doRetrieveMatchingFiles 2.4 问题总结 3. 扫描De ...

ros ur5模拟扫描

ros ur5模拟扫描相关推荐

最新文章

热门文章