def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('get_camera_pose_image_auto')
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
arm.set_max_velocity_scaling_factor(0.8)
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_planning_time(0.5) # 规划时间限制为2秒
arm.allow_replanning(False) # 当运动规划失败后，是否允许重新规划
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
# 开始图像接收
image_receiver = ImageReceiver()
self.group = arm
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.listener = tf.TransformListener()
self.save_cnt = 0
print "\n\n[INFO] Pose image saver started."
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
# arm.set_goal_position_tolerance(0.00001)
# arm.set_goal_orientation_tolerance(0.00001)
arm.set_max_velocity_scaling_factor(0.5)
arm.set_max_acceleration_scaling_factor(1.0)
arm.set_planning_time(0.5) # 规划时间限制为2秒
arm.allow_replanning(True) # 当运动规划失败后，是否允许重新规划
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
self.group = arm
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.listener = tf.TransformListener()
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')
robot = moveit_commander.RobotCommander()
# 当运动规划失败后，允许重新规划
arm.allow_replanning(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.01)
arm.set_max_velocity_scaling_factor(0.5)
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_planning_time(0.08) # 规划时间限制为2秒
# arm.set_num_planning_attempts(1) # 规划1次
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
scene = moveit_commander.PlanningSceneInterface()
scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
print "[INFO] Current pose:\n", arm.get_current_pose().pose
self.scene = scene
self.scene_pub = scene_pub
self.colors = dict()
self.group = arm
self.robot = robot
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.broadcaster = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
self.gripper_len = 0.095 # 手爪实际长度0.165m, 虚拟夹爪深度0.075m 0.16-0.065=0.095m
self.approach_distance = 0.06
self.back_distance = 0.05
# sub and pub point cloud
self.point_cloud = None
self.update_cloud_flag = False
rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.callback_pointcloud)
thread.start_new_thread(self.publish_pointcloud, ())
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.01)
arm.set_max_velocity_scaling_factor(0.8)
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_planning_time(1) # 规划时间限制为2秒
arm.set_num_planning_attempts(2) # 规划两次
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
scene = moveit_commander.PlanningSceneInterface()
print arm.get_current_pose(eef_link).pose
sub = rospy.Subscriber('/detect_grasps_yolo/juggle_rects',
Float64MultiArray, self.callback)
self.juggle_rects = Float64MultiArray()
self.box_name = ''
self.scene = scene
self.group = arm
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.move_distance = 0.1
self.back_distance = 0.15
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
robot = moveit_commander.RobotCommander()
# 当运动规划失败后，允许重新规划
arm.allow_replanning(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.0001)
arm.set_goal_orientation_tolerance(0.0001)
arm.set_max_velocity_scaling_factor(0.5)
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
scene = moveit_commander.PlanningSceneInterface()
print "[INFO] Current pose:", arm.get_current_pose(eef_link).pose
# 控制机械臂运动到之前设置的姿态
# arm.set_named_target('pick_6')
# arm.set_named_target('home')
# arm.go()
self.box_name = ''
self.scene = scene
self.group = arm
self.robot = robot
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.move_distance = 0.1
self.back_distance = 0.15
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(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.01)
arm.set_max_velocity_scaling_factor(0.4)
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_planning_time(0.1) # 规划时间限制为2秒
# arm.set_num_planning_attempts(1) # 规划1次
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
scene = moveit_commander.PlanningSceneInterface()
print "[INFO] Current pose:\n", arm.get_current_pose().pose
self.box_name = ''
self.scene = scene
self.group = arm
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.broadcaster = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
self.gripper_len = 0.082 # 手爪实际长度0.165m, 虚拟夹爪深度0.075m 0.16-0.075=0.085m
self.approach_distance = 0.05
self.back_distance = 0.05
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('gripper_test')
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
arm.set_max_velocity_scaling_factor(0.3)
arm.set_max_acceleration_scaling_factor(0.5)
# 初始化需要使用move group控制的机械臂中的gripper group
gripper = MoveGroupCommander('gripper')
self.gripper = gripper
# 控制机械臂先回到初始位置
arm.set_named_target('home')
arm.go()
# 控制夹爪张开
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(0.5)
# 控制机械臂到目标位置
arm.set_named_target('test_2')
arm.go()
# 控制夹爪闭合
gripper.set_joint_value_target(GRIPPER_CLOSED)
gripper.go()
# 控制机械臂先回到初始位置
arm.set_named_target('home')
arm.go()
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def init_mp(cls):
movegroup_ns = ManipulatorActions.get_ns()
robot_description = ManipulatorActions.get_robdesc_ns()
sc = PlanningSceneInterface(ns=movegroup_ns)
mg = MoveGroupCommander('arm',
ns=movegroup_ns,
robot_description=robot_description)
mg.set_max_acceleration_scaling_factor(0.5)
mg.set_max_velocity_scaling_factor(0.6)
mg.set_end_effector_link('tool_frame')
rospy.sleep(1.0)
sc.remove_world_object()
ManipulatorActions.rc = RobotCommander(ns=movegroup_ns)
ManipulatorActions.mg = mg
ManipulatorActions.sc = sc
ManipulatorActions.add_table_plane()
ManipulatorActions.measure_weight(calibrate=True)
ManipulatorActions.move_up(home=True)
ManipulatorActions.measure_weight(calibrate=True)
return mg, sc
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')
robot = moveit_commander.RobotCommander()
# 当运动规划失败后，允许重新规划
arm.allow_replanning(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.0001)
arm.set_goal_orientation_tolerance(0.0001)
arm.set_max_velocity_scaling_factor(1.0)
arm.set_planning_time(0.05) # 规划时间限制
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
print "[INFO] Current pose:", arm.get_current_pose(eef_link).pose
# 控制机械臂运动到之前设置的姿态
arm.set_named_target('work')
arm.go()
self.group = arm
self.robot = robot
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
def init():
global marker_array_pub, marker_pub, tf_broadcaster, tf_listener
global move_group, turntable
global camera_mesh, camera_pos, camera_size, min_distance, max_distance, num_positions, num_spins, object_size, photobox_pos, photobox_size, reach, simulation, test, turntable_pos, working_dir
rospy.init_node('acquisition')
camera_mesh = rospy.get_param('~camera_mesh', None)
camera_orientation = rospy.get_param('~camera_orientation', None)
camera_pos = rospy.get_param('~camera_pos', [0.0, 0.0, 0.0])
camera_size = rospy.get_param('~camera_size', [0.1, 0.1, 0.1])
min_distance = rospy.get_param('~min_distance', 0.2)
max_distance = rospy.get_param('~max_distance', min_distance)
max_velocity = rospy.get_param('~max_velocity', 0.1)
num_positions = rospy.get_param('~num_positions', 15)
num_spins = rospy.get_param('~num_spins', 8)
object_size = numpy.array(rospy.get_param('~object_size', [0.2, 0.2, 0.2]))
photobox_pos = rospy.get_param('~photobox_pos', [0.0, -0.6, 0.0])
photobox_size = rospy.get_param('~photobox_size', [0.7, 0.7, 1.0])
ptpip = rospy.get_param('~ptpip', None)
reach = rospy.get_param('~reach', 0.85)
simulation = '/gazebo' in get_node_names()
test = rospy.get_param('~test', True)
turntable_pos = rospy.get_param(
'~turntable_pos', photobox_pos[:2] + [photobox_pos[2] + 0.05])
turntable_radius = rospy.get_param('~turntable_radius', 0.2)
wall_thickness = rospy.get_param('~wall_thickness', 0.04)
marker_array_pub = rospy.Publisher('visualization_marker_array',
MarkerArray,
queue_size=1,
latch=True)
marker_pub = rospy.Publisher('visualization_marker',
Marker,
queue_size=1,
latch=True)
tf_broadcaster = tf.TransformBroadcaster()
tf_listener = tf.TransformListener()
move_group = MoveGroupCommander('manipulator')
move_group.set_max_acceleration_scaling_factor(
1.0 if simulation else max_velocity)
move_group.set_max_velocity_scaling_factor(
1.0 if simulation else max_velocity)
robot = RobotCommander()
scene = PlanningSceneInterface(synchronous=True)
try:
st_control = load_source(
'st_control',
os.path.join(RosPack().get_path('iai_scanning_table'), 'scripts',
'iai_scanning_table', 'st_control.py'))
turntable = st_control.ElmoUdp()
if turntable.check_device():
turntable.configure()
turntable.reset_encoder()
turntable.start_controller()
turntable.set_speed_deg(30)
except Exception as e:
print(e)
sys.exit('Could not connect to turntable.')
if simulation:
move_home()
elif not test:
working_dir = rospy.get_param('~working_dir', None)
if working_dir is None:
sys.exit('Working directory not specified.')
elif not camera.init(
os.path.join(os.path.dirname(os.path.abspath(__file__)), '..',
'out', working_dir, 'images'), ptpip):
sys.exit('Could not initialize camera.')
# add ground plane
ps = PoseStamped()
ps.header.frame_id = robot.get_planning_frame()
scene.add_plane('ground', ps)
# add photobox
ps.pose.position.x = photobox_pos[
0] + photobox_size[0] / 2 + wall_thickness / 2
ps.pose.position.y = photobox_pos[1]
ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2
scene.add_box('box_wall_left', ps,
(wall_thickness, photobox_size[1], photobox_size[2]))
ps.pose.position.x = photobox_pos[
0] - photobox_size[0] / 2 - wall_thickness / 2
ps.pose.position.y = photobox_pos[1]
ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2
scene.add_box('box_wall_right', ps,
(wall_thickness, photobox_size[1], photobox_size[2]))
ps.pose.position.x = photobox_pos[0]
ps.pose.position.y = photobox_pos[
1] - photobox_size[1] / 2 - wall_thickness / 2
ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2
scene.add_box('box_wall_back', ps,
(photobox_size[0], wall_thickness, photobox_size[2]))
ps.pose.position.x = photobox_pos[0]
ps.pose.position.y = photobox_pos[1]
ps.pose.position.z = photobox_pos[2] - wall_thickness / 2
scene.add_box('box_ground', ps,
(photobox_size[0], photobox_size[1], wall_thickness))
# add turntable
turntable_height = turntable_pos[2] - photobox_pos[2]
ps.pose.position.x = turntable_pos[0]
ps.pose.position.y = turntable_pos[1]
ps.pose.position.z = photobox_pos[2] + turntable_height / 2
scene.add_cylinder('turntable', ps, turntable_height, turntable_radius)
# add object on turntable
ps.pose.position.x = turntable_pos[0]
ps.pose.position.y = turntable_pos[1]
ps.pose.position.z = turntable_pos[2] + object_size[2] / 2
scene.add_cylinder('object', ps, object_size[2], max(object_size[:2]) / 2)
# add cable mounts
scene.remove_attached_object('upper_arm_link', 'upper_arm_cable_mount')
scene.remove_attached_object('forearm_link', 'forearm_cable_mount')
scene.remove_world_object('upper_arm_cable_mount')
scene.remove_world_object('forearm_cable_mount')
if ptpip is None and not simulation:
size = [0.08, 0.08, 0.08]
ps.header.frame_id = 'upper_arm_link'
ps.pose.position.x = -0.13
ps.pose.position.y = -0.095
ps.pose.position.z = 0.135
scene.attach_box(ps.header.frame_id, 'upper_arm_cable_mount', ps, size)
ps.header.frame_id = 'forearm_link'
ps.pose.position.x = -0.275
ps.pose.position.y = -0.08
ps.pose.position.z = 0.02
scene.attach_box(ps.header.frame_id, 'forearm_cable_mount', ps, size)
# add camera
eef_link = move_group.get_end_effector_link()
ps = PoseStamped()
ps.header.frame_id = eef_link
scene.remove_attached_object(eef_link, 'camera_0')
scene.remove_attached_object(eef_link, 'camera_1')
scene.remove_world_object('camera_0')
scene.remove_world_object('camera_1')
ps.pose.position.y = camera_pos[1]
ps.pose.position.z = camera_pos[2]
if camera_mesh:
ps.pose.position.x = camera_pos[0]
quaternion = tf.transformations.quaternion_from_euler(
math.radians(camera_orientation[0]),
math.radians(camera_orientation[1]),
math.radians(camera_orientation[2]))
ps.pose.orientation.x = quaternion[0]
ps.pose.orientation.y = quaternion[1]
ps.pose.orientation.z = quaternion[2]
ps.pose.orientation.w = quaternion[3]
scene.attach_mesh(eef_link, 'camera_0', ps,
os.path.expanduser(camera_mesh), camera_size)
if not simulation:
scene.attach_mesh(eef_link, 'camera_1', ps,
os.path.expanduser(camera_mesh),
numpy.array(camera_size) * 1.5)
vertices = scene.get_attached_objects(
['camera_0'])['camera_0'].object.meshes[0].vertices
camera_size[0] = max(vertice.x for vertice in vertices) - min(
vertice.x for vertice in vertices)
camera_size[1] = max(vertice.y for vertice in vertices) - min(
vertice.y for vertice in vertices)
camera_size[2] = max(vertice.z for vertice in vertices) - min(
vertice.z for vertice in vertices)
else:
ps.pose.position.x = camera_pos[0] + camera_size[0] / 2
scene.attach_box(eef_link, 'camera_0', ps, camera_size)
def inverse_kinematics():
# Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "right_arm"
request.ik_request.ik_link_name = "right_gripper"
request.ik_request.attempts = 50
request.ik_request.pose_stamped.header.frame_id = "base"
# Create joint constraints
#This is joint constraint will need to be set at the group
constraints = Constraints()
joint_constr = JointConstraint()
joint_constr.joint_name = "right_j0"
joint_constr.position = TORSO_POSITION
joint_constr.tolerance_above = TOLERANCE
joint_constr.tolerance_below = TOLERANCE
joint_constr.weight = 0.5
constraints.joint_constraints.append(joint_constr)
# Get the transformed AR Tag (x,y,z) coordinates
# Only care about the x coordinate of AR tag; tells use
# how far away wall is
# x,y, z tell us the origin of the AR Tag
x_coord = board_x # DONT CHANGE
y_coord = bounding_points[0]
z_coord = bounding_points[1]
y_width = bounding_points[2]
z_height = bounding_points[3]
#Creating Path Planning
waypoints = []
z_bais = 0
print(
"OMMMMMMMMMMMMMMMMMMMMMMMMMGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGG!!!!!!!!!!!!!!"
)
print(bounding_points)
for i in range(int(float(z_height) / .03)):
target_pose = Pose()
target_pose.position.x = float(x_coord - PLANNING_BIAS)
if i % 2 == 0:
#Starting positions
target_pose.position.y = float(y_coord)
else:
target_pose.position.y = y_coord + float(y_width)
#Starting positions
target_pose.position.z = float(z_coord + z_bais)
target_pose.orientation.y = 1.0 / 2**(1 / 2.0)
target_pose.orientation.w = 1.0 / 2**(1 / 2.0)
waypoints.append(target_pose)
z_bais += .03
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose = target_pose
try:
#Send the request to the service
response = compute_ik(request)
group = MoveGroupCommander("right_arm")
group.set_max_velocity_scaling_factor(0.75)
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose = target_pose
#Creating a Robot Trajectory for the Path Planning
jump_thres = 0.0
eef_step = 0.1
path, fraction = group.compute_cartesian_path([target_pose],
eef_step, jump_thres)
print("Path fraction: {}".format(fraction))
#Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
#Setting the Joint constraint
group.set_path_constraints(constraints)
if fraction < 0.5:
group.go()
else:
group.execute(path)
if i < int(float(z_height) / 0.03) and i > 0:
target2 = target_pose
target2.position.z += 0.03
path, fraction = group.compute_cartesian_path([target2],
eef_step,
jump_thres)
group.set_path_constraints(constraints)
group.execute(path)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
class CalibrationMovements:
def __init__(self,
move_group_name,
max_velocity_scaling,
max_acceleration_scaling,
angle_delta,
translation_delta,
move_group_namespace='/'):
# self.client = HandeyeClient() # TODO: move around marker when eye_on_hand, automatically take samples via trigger topic
if not move_group_namespace.endswith('/'):
move_group_namespace = move_group_namespace + '/'
if move_group_namespace != '/':
self.mgc = MoveGroupCommander(
move_group_name,
robot_description=move_group_namespace + 'robot_description',
ns=move_group_namespace)
else:
self.mgc = MoveGroupCommander(move_group_name)
self.mgc.set_planner_id("RRTConnectkConfigDefault"
) # TODO: this is only needed for the UR5
self.mgc.set_max_velocity_scaling_factor(max_velocity_scaling)
self.mgc.set_max_acceleration_scaling_factor(max_acceleration_scaling)
self.start_pose = self.mgc.get_current_pose()
self.joint_limits = [math.radians(90)] * 5 + [math.radians(180)] + [
math.radians(350)
] # TODO: make param
self.target_poses = []
self.current_pose_index = None
self.current_plan = None
self.fallback_joint_limits = [math.radians(90)] * 4 + [
math.radians(90)
] + [math.radians(180)] + [math.radians(350)]
if len(self.mgc.get_active_joints()) == 6:
self.fallback_joint_limits = self.fallback_joint_limits[1:]
self.angle_delta = angle_delta
self.translation_delta = translation_delta
def set_and_check_starting_position(self):
# sets the starting position to the current robot cartesian EE position and checks movement in all directions
# TODO: make repeatable
# - save the home position and each target position as joint position
# - plan to each target position and back, save plan if not crazy
# - return true if can plan to each target position without going crazy
self.start_pose = self.mgc.get_current_pose()
self.target_poses = self._compute_poses_around_state(
self.start_pose, self.angle_delta, self.translation_delta)
self.current_pose_index = -1
ret = self._check_target_poses(self.joint_limits)
if ret:
rospy.loginfo("Set current pose as home")
return True
else:
rospy.logerr("Can't calibrate from this position!")
self.start_pose = None
self.target_poses = None
return False
def select_target_pose(self, i):
number_of_target_poses = len(self.target_poses)
if 0 <= i < number_of_target_poses:
rospy.loginfo("Selected pose {} for next movement".format(i))
self.current_pose_index = i
return True
else:
rospy.logerr(
"Index {} is out of bounds: there are {} target poses".format(
i, number_of_target_poses))
return False
def plan_to_start_pose(self):
# TODO: use joint position http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html#planning-to-a-joint-goal
rospy.loginfo("Planning to home pose")
return self._plan_to_pose(self.start_pose)
def plan_to_current_target_pose(self):
# TODO: use joint position http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html#planning-to-a-joint-goal
i = self.current_pose_index
rospy.loginfo("Planning to target pose {}".format(i))
return self._plan_to_pose(self.target_poses[i])
def execute_plan(self):
if self.plan is None:
rospy.logerr("No plan found!")
return False
if CalibrationMovements._is_crazy_plan(self.plan,
self.fallback_joint_limits):
rospy.logerr("Crazy plan found, not executing!")
return False
self.mgc.execute(self.plan)
return True
def _plan_to_pose(self, pose):
self.mgc.set_start_state_to_current_state()
self.mgc.set_pose_target(pose)
ret = self.mgc.plan()
if type(ret) is tuple:
# noetic
success, plan, planning_time, error_code = ret
else:
# melodic
plan = ret
if CalibrationMovements._is_crazy_plan(plan,
self.fallback_joint_limits):
rospy.logwarn("Planning failed")
self.plan = None
return False
else:
rospy.loginfo("Planning successful")
self.plan = plan
return True
def _check_target_poses(self, joint_limits):
if len(self.fallback_joint_limits) == 6:
joint_limits = joint_limits[1:]
for fp in self.target_poses:
self.mgc.set_pose_target(fp)
ret = self.mgc.plan()
if type(ret) is tuple:
# noetic
success, plan, planning_time, error_code = ret
else:
# melodic
plan = ret
if len(plan.joint_trajectory.points
) == 0 or CalibrationMovements._is_crazy_plan(
plan, joint_limits):
return False
return True
@staticmethod
def _compute_poses_around_state(start_pose, angle_delta,
translation_delta):
basis = np.eye(3)
pos_deltas = [
quaternion_from_euler(*rot_axis * angle_delta)
for rot_axis in basis
]
neg_deltas = [
quaternion_from_euler(*rot_axis * (-angle_delta))
for rot_axis in basis
]
quaternion_deltas = list(
chain.from_iterable(zip(pos_deltas, neg_deltas))) # interleave
final_rots = []
for qd in quaternion_deltas:
final_rots.append(list(qd))
# TODO: accept a list of delta values
pos_deltas = [
quaternion_from_euler(*rot_axis * angle_delta / 2)
for rot_axis in basis
]
neg_deltas = [
quaternion_from_euler(*rot_axis * (-angle_delta / 2))
for rot_axis in basis
]
quaternion_deltas = list(
chain.from_iterable(zip(pos_deltas, neg_deltas))) # interleave
for qd in quaternion_deltas:
final_rots.append(list(qd))
final_poses = []
for rot in final_rots:
fp = deepcopy(start_pose)
ori = fp.pose.orientation
combined_rot = quaternion_multiply([ori.x, ori.y, ori.z, ori.w],
rot)
fp.pose.orientation = Quaternion(*combined_rot)
final_poses.append(fp)
fp = deepcopy(start_pose)
fp.pose.position.x += translation_delta / 2
final_poses.append(fp)
fp = deepcopy(start_pose)
fp.pose.position.x -= translation_delta / 2
final_poses.append(fp)
fp = deepcopy(start_pose)
fp.pose.position.y += translation_delta
final_poses.append(fp)
fp = deepcopy(start_pose)
fp.pose.position.y -= translation_delta
final_poses.append(fp)
fp = deepcopy(start_pose)
fp.pose.position.z += translation_delta / 3
final_poses.append(fp)
return final_poses
@staticmethod
def _rot_per_joint(plan, degrees=False):
np_traj = np.array([p.positions for p in plan.joint_trajectory.points])
if len(np_traj) == 0:
raise ValueError
np_traj_max_per_joint = np_traj.max(axis=0)
np_traj_min_per_joint = np_traj.min(axis=0)
ret = abs(np_traj_max_per_joint - np_traj_min_per_joint)
if degrees:
ret = [math.degrees(j) for j in ret]
return ret
@staticmethod
def _is_crazy_plan(plan, max_rotation_per_joint):
abs_rot_per_joint = CalibrationMovements._rot_per_joint(plan)
if (abs_rot_per_joint > max_rotation_per_joint).any():
return True
else:
return False
class MoveItObstaclesDemo:
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_obstacles_demo')
# 等待场景准备就绪
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
self.arm = MoveGroupCommander('manipulator')
# 设置机械臂工作空间
self.arm.set_workspace([-100, -100, 0, 100, 0.3, 100])
# 设置机械臂最大速度
self.arm.set_max_velocity_scaling_factor(value=0.1)
# 获取终端link的名称
self.end_effector_link = self.arm.get_end_effector_link()
rospy.loginfo('end effector link: {}'.format(self.end_effector_link))
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
# self.arm.allow_replanning(True)
self.arm.set_num_planning_attempts(10)
# self.arm.allow_looking(True)
# 设置目标位置所使用的参考坐标系
self.reference_frame = 'base_link'
self.arm.set_pose_reference_frame(self.reference_frame)
# 设置每次运动规划的时间限制：5s
self.arm.set_planning_time(3)
# 控制机械臂先回到初始化位置
self.arm.set_named_target('home')
self.arm.go()
rospy.sleep(2)
def planning(self, start_point, end_point):
"""
功能：动态避障
:param start_point: 起始点, type: dict
:param end_point: 终点, type: dict
:return: None
"""
# 移动机械臂到指定位置，并获取当前位姿数据为机械臂运动的起始位姿
if start_point:
self.move_arm(p=start_point)
self.start_pose = self.arm.get_current_pose(self.end_effector_link).pose
# 使用moveit自带的求解器规划出A到B的离散路径点, 并存到列表way_points中
# way_points = self.get_way_points(start_point, end_point)
while True:
print('set planner id: RRT')
self.arm.set_planner_id('TRRTkConfigDefault')
self.arm.set_named_target('up')
self.arm.go()
rospy.sleep(5)
print('set planner id: PRM')
self.arm.set_planner_id('TRRTkConfigDefault')
self.arm.set_named_target('home')
self.arm.go()
rospy.sleep(5)
# -------------------------------------------------------------------
def get_way_points(self, a, b):
way_points = []
# plan 1
self.arm.set_named_target('up')
traj = self.arm.plan()
if traj.joint_trajectory.points: # True if trajectory contains points
rospy.loginfo("get trajectory success")
else:
rospy.logerr("Trajectory is empty. Planning false!")
self.arm.clear_pose_targets()
# plan 2
# traj = self.moveit_planning(p=b)
for i, p in enumerate(traj.joint_trajectory.points):
# rospy.loginfo('waypoint: {}'.format(i))
if i%2 == 0:
point = {
'x': p.positions[0],
'y': p.positions[1],
'z': p.positions[2],
'ox': p.positions[3],
'oy': p.positions[4],
'oz': p.positions[5]
}
way_points.append(point)
rospy.loginfo('waypoint: \n {}'.format(way_points))
rospy.loginfo(len(way_points))
return way_points
def moveit_planning(self, p):
"""
使用moveit求解器规划路径
:param p: dict, e.g., {'x': 0, 'y': 0, 'z': 0, 'ox': 0, 'oy': 0, 'oz': 0, 'ow': 0}
:return:
"""
rospy.loginfo('start moveit planning...')
target_pose = PoseStamped()
target_pose.header.frame_id = self.reference_frame
target_pose.pose.position.x = p['x']
target_pose.pose.position.y = p['y']
target_pose.pose.position.z = p['z']
if 'ox' in p.keys() and p['ox']:
target_pose.pose.orientation.x = p['ox']
if 'oy' in p.keys() and p['oy']:
target_pose.pose.orientation.y = p['oy']
if 'oz' in p.keys() and p['oz']:
target_pose.pose.orientation.z = p['oz']
if 'ow' in p.keys() and p['ow']:
target_pose.pose.orientation.w = p['ow']
# 传入一个PoseStamped
# self.arm.set_pose_target(target_pose, self.end_effector_link)
# 尝试直接传入一个列表
self.arm.set_pose_target([p['x'], p['y'], p['z'], p['ox'], p['oy'], p['oz']], self.end_effector_link)
traj = self.arm.plan()
if traj.joint_trajectory.points: # True if trajectory contains points
rospy.loginfo("get trajectory success")
return traj
else:
rospy.logerr("Trajectory is empty. Planning false!")
def move_arm(self, p):
"""
移动机械臂到p点
:param p: dict, e.g., {'x': 0, 'y': 0, 'z': 0, 'ox': 0, 'oy': 0, 'oz': 0, 'ow': 0}
:return:
"""
rospy.loginfo('start arm moving...')
traj = self.moveit_planning(p)
self.arm.execute(traj)
# 设置当前位置为起始位置
self.arm.set_start_state_to_current_state()
rospy.sleep(1)
def stop_arm(self):
"""
急停
:return:
"""
pass
def exist_danger_obstacle(self):
"""
环境中是否存在危险的障碍物
:return: True or False
"""
return False
def get_obstacle_octomap(self):
"""
获取环境的octomap信息
:return:
"""
pass
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划，获取参数，如果没有设置，则默认为True，即走直线
cartesian = True #rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('armc_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('init_pose')
arm.go()
rospy.sleep(1)
# 设置机械臂运动的起始位姿
joint_goal = arm.get_current_joint_values()
joint_goal[0] = 0
joint_goal[1] = -pi / 6
joint_goal[2] = 0
joint_goal[3] = pi / 3
joint_goal[4] = 0
joint_goal[5] = pi / 3
joint_goal[6] = 0
arm.go(joint_goal)
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.x += 0.1
waypoints.append(deepcopy(wpose))
if cartesian: #如果设置为True，那么走直线
#wpose.position.x += 0.020
waypoints.append(deepcopy(wpose))
else: #否则就走曲线
arm.set_pose_target(wpose) #自由曲线
arm.go()
rospy.sleep(1)
wpose.position.x += 0.01
if cartesian:
#wpose.position.x += 0.030
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
wpose.position.y += 0.1
if cartesian:
#wpose.position.x += 0.040
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:
#wpose.position.x += 0.050
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(5)
# 控制机械臂先回到初始化位置
#arm.set_named_target('init_pose')
#arm.go()
#rospy.sleep(5)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_drawing', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.01)
# 设置允许的最大速度和加速度
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)
star_pose = PoseStamped()
star_pose.header.frame_id = reference_frame
star_pose.header.stamp = rospy.Time.now()
star_pose.pose.position.x = 0.40
star_pose.pose.position.y = 0.0
star_pose.pose.position.z = 0.12
star_pose.pose.orientation.w = 1.0
# 设置机械臂终端运动的目标位姿
arm.set_pose_target(star_pose, end_effector_link)
arm.go()
radius = 0.1
centerY = 0.0
centerX = 0.40 - radius
# 初始化路点列表
waypoints = []
starPoints = []
pose_temp = star_pose
for th in numpy.arange(0, 6.2831855, 1.2566371):
pose_temp.pose.position.y = -(centerY + radius * math.sin(th))
pose_temp.pose.position.x = centerX + radius * math.cos(th)
pose_temp.pose.position.z = 0.113
wpose = deepcopy(pose_temp.pose)
starPoints.append(deepcopy(wpose))
# 将圆弧上的路径点加入列表
waypoints.append(starPoints[0])
waypoints.append(starPoints[2])
waypoints.append(starPoints[4])
waypoints.append(starPoints[1])
waypoints.append(starPoints[3])
waypoints.append(starPoints[0])
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)
def __init__(self, pickPos, placePos):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
pick_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the momitive group for the right gripper
pick_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = pick_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
pick_arm.set_goal_position_tolerance(0.05)
pick_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
pick_arm.allow_replanning(True)
# Set the right arm reference frame
pick_arm.set_pose_reference_frame(REFERENCE_FRAME)
#pick_arm.set_planner_id("RRTConnectkConfigDefault")
#pick_arm.set_planner_id("RRTstarkConfigDefault")
pick_arm.set_planner_id("RRTstarkConfigDefault")
# Allow 5 seconds per planning attempt
pick_arm.set_planning_time(3)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
base_table_id = 'base_table'
target_id = 'target'
limit_table_id = 'limit_table'
#tool_id = 'tool'
# Remove leftover objects from a previous run
scene.remove_world_object(base_table_id)
#scene.remove_world_object(table_id)
#scene.remove_world_object(box1_id)
#scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
#scene.remove_world_object(tool_id)
scene.remove_world_object(limit_table_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Open the gripper to the neutral position
# pick_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
# pick_gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
# table_ground = 0.6
table_ground = 0.0
# Set the dimensions of the scene objects [l, w, h]
base_table_size = [2, 2, 0.04]
#table_size = [0.2, 0.7, 0.01]
#box1_size = [0.1, 0.05, 0.05]
#box2_size = [0.05, 0.05, 0.15]
# Set the target size [l, w, h]
target_size = [0.055, 0.055, 0.12]
target_radius = 0.03305
target_height = 0.12310
limit_table_size = [0.6, 1.8, 0.04]
# Add a base table to the scene
base_table_pose = PoseStamped()
base_table_pose.header.frame_id = REFERENCE_FRAME
base_table_pose.pose.position.x = 0.0
base_table_pose.pose.position.y = 0.0
base_table_pose.pose.position.z = -0.3
base_table_pose.pose.orientation.w = 1.0
scene.add_box(base_table_id, base_table_pose, base_table_size)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
pick_arm.set_named_target(ARM_HOME_POSE)
pick_arm.go()
rospy.sleep(1)
# Add a table top and two boxes to the scene
#table_pose = PoseStamped()
#table_pose.header.frame_id = REFERENCE_FRAME
#table_pose.pose.position.x = 0.5
#table_pose.pose.position.y = -0.4
#table_pose.pose.position.z = table_ground + table_size[2] / 2.0
#table_pose.pose.orientation.w = 1.0
#scene.add_box(table_id, table_pose, table_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = pickPos.pose.position.x
target_pose.pose.position.y = pickPos.pose.position.y
target_pose.pose.position.z = pickPos.pose.position.z
target_pose.pose.orientation.w = pickPos.pose.orientation.w
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
#scene.add_cylinder(target_id,target_pose,target_height,target_radius)
#Add the limit_table object to the scene
limit_table_pose = PoseStamped()
limit_table_pose.header.frame_id = REFERENCE_FRAME
limit_table_pose.pose.position.x = 0.58
limit_table_pose.pose.position.y = -0.4
limit_table_pose.pose.position.z = 0.18
limit_table_pose.pose.orientation.w = 1.0
#scene.add_box(limit_table_id, limit_table_pose, limit_table_size)
self.setColor(limit_table_id, 0.6, 0.2, 0.2, 1.0)
# Make the table red and the boxes orange
#self.setColor(table_id, 0.8, 0, 0, 1.0)
# self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
# self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
#pick_arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
#指定拾取后的放置目标姿态
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = placePos.pose.position.x
place_pose.pose.position.y = placePos.pose.position.y
place_pose.pose.position.z = placePos.pose.position.z
place_pose.pose.orientation.w = placePos.pose.orientation.w
place_pose.pose.orientation.x = placePos.pose.orientation.x
place_pose.pose.orientation.y = placePos.pose.orientation.y
place_pose.pose.orientation.z = placePos.pose.orientation.z
#设置机器人运行最大速度位百分之～
pick_arm.set_max_velocity_scaling_factor(0.5)
# Initialize the grasp pose to the target pose
#初始化抓取目标点位
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
# this way is just used by PR2 robot
#grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RV
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
#重复直到成功或者超出尝试的次数
#while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
while result != 1 and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
#moveit中的pick接口，target_id为moveit添加场景的id，此处为目标物体，grasps为可尝试抓取的点位序列
result = pick_arm.pick(target_id, grasps)
#打印信息
rospy.logerr("pick_arm.pick： " + str(result))
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
#如果抓取成功，尝试放置操作
result_g = None
n_attempts = 2
#if result == MoveItErrorCodes.SUCCESS:
while result_g != True and n_attempts < max_pick_attempts: #and result == 1:
# Generate valid place poses
#places = self.make_places(place_pose)
n_attempts += 1
print("-------------------")
print(place_pose)
#更新当前的机械臂状态
pick_arm.set_start_state_to_current_state()
#设置moveit运动的目标点位
pick_arm.set_pose_target(place_pose)
# Repeat until we succeed or run out of attempts
#while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
# n_attempts += 1
# rospy.loginfo("Place attempt: " + str(n_attempts))
# for place in places:
# result = pick_arm.place(target_id, place)
# if result == MoveItErrorCodes.SUCCESS:
# break
# rospy.sleep(0.2)
#moveit的运动接口，wait=True表示等到执行完成才返回
result_g = pick_arm.go(wait=True)
rospy.logerr("pick_arm.go： " + str(result_g))
#打开夹爪
open_client(500)
rospy.sleep(0.2)
#if result == "False"
# rospy.logerr("Place operation failed after " + str(n_attempts) + " attempts.")
#else:
# rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
# Return the arm to the "home" pose stored in the SRDF file
#将机械臂返回到SRDF中的“home”姿态
#scene.remove_world_object(target_id)
#scene.remove_attached_object(GRIPPER_FRAME, target_id)
#scene.remove_world_object(target_id)
rospy.sleep(1)
open_client(500)
pick_arm.set_named_target(ARM_HOME_POSE)
pick_arm.go()
# Open the gripper to the neutral position
# pick_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
# pick_gripper.go()
rospy.sleep(1)
rospy.logerr("pick_server over ")
class CalibrationMovements:
def __init__(self,
move_group_name,
max_velocity_scaling=0.5,
max_acceleration_scaling=0.5):
#self.client = HandeyeClient() # TODO: move around marker when eye_on_hand, automatically take samples via trigger topic
self.mgc = MoveGroupCommander(move_group_name)
self.mgc.set_planner_id("RRTConnectkConfigDefault")
self.mgc.set_max_velocity_scaling_factor(max_velocity_scaling)
self.mgc.set_max_acceleration_scaling_factor(max_acceleration_scaling)
self.start_pose = self.mgc.get_current_pose()
self.poses = []
self.current_pose_index = -1
self.fallback_joint_limits = [math.radians(90)] * 4 + [
math.radians(90)
] + [math.radians(180)] + [math.radians(350)]
if len(self.mgc.get_active_joints()) == 6:
self.fallback_joint_limits = self.fallback_joint_limits[1:]
def compute_poses_around_current_state(self, angle_delta,
translation_delta):
self.start_pose = self.mgc.get_current_pose()
basis = np.eye(3)
pos_deltas = [
quaternion_from_euler(*rot_axis * angle_delta)
for rot_axis in basis
]
neg_deltas = [
quaternion_from_euler(*rot_axis * (-angle_delta))
for rot_axis in basis
]
quaternion_deltas = list(
chain.from_iterable(izip(pos_deltas, neg_deltas))) # interleave
final_rots = []
for qd in quaternion_deltas:
final_rots.append(list(qd))
# TODO: clean up
pos_deltas = [
quaternion_from_euler(*rot_axis * angle_delta / 2)
for rot_axis in basis
]
neg_deltas = [
quaternion_from_euler(*rot_axis * (-angle_delta / 2))
for rot_axis in basis
]
quaternion_deltas = list(
chain.from_iterable(izip(pos_deltas, neg_deltas))) # interleave
for qd in quaternion_deltas:
final_rots.append(list(qd))
final_poses = []
for rot in final_rots:
fp = deepcopy(self.start_pose)
ori = fp.pose.orientation
combined_rot = quaternion_multiply([ori.x, ori.y, ori.z, ori.w],
rot)
fp.pose.orientation = Quaternion(*combined_rot)
final_poses.append(fp)
fp = deepcopy(self.start_pose)
fp.pose.position.x += translation_delta / 2
final_poses.append(fp)
fp = deepcopy(self.start_pose)
fp.pose.position.x -= translation_delta / 2
final_poses.append(fp)
fp = deepcopy(self.start_pose)
fp.pose.position.y += translation_delta
final_poses.append(fp)
fp = deepcopy(self.start_pose)
fp.pose.position.y -= translation_delta
final_poses.append(fp)
fp = deepcopy(self.start_pose)
fp.pose.position.z += translation_delta / 3
final_poses.append(fp)
self.poses = final_poses
self.current_pose_index = -1
def check_poses(self, joint_limits):
if len(self.fallback_joint_limits) == 6:
joint_limits = joint_limits[1:]
for fp in self.poses:
self.mgc.set_pose_target(fp)
plan = self.mgc.plan()
if len(plan.joint_trajectory.points
) == 0 or CalibrationMovements.is_crazy_plan(
plan, joint_limits):
return False
return True
def plan_to_start_pose(self):
return self.plan_to_pose(self.start_pose)
def plan_to_pose(self, pose):
self.mgc.set_start_state_to_current_state()
self.mgc.set_pose_target(pose)
plan = self.mgc.plan()
return plan
def execute_plan(self, plan):
if CalibrationMovements.is_crazy_plan(plan,
self.fallback_joint_limits):
raise RuntimeError("got crazy plan!")
self.mgc.execute(plan)
@staticmethod
def rot_per_joint(plan, degrees=False):
np_traj = np.array([p.positions for p in plan.joint_trajectory.points])
if len(np_traj) == 0:
raise ValueError
np_traj_max_per_joint = np_traj.max(axis=0)
np_traj_min_per_joint = np_traj.min(axis=0)
ret = abs(np_traj_max_per_joint - np_traj_min_per_joint)
if degrees:
ret = [math.degrees(j) for j in ret]
return ret
@staticmethod
def is_crazy_plan(plan, max_rotation_per_joint):
abs_rot_per_joint = CalibrationMovements.rot_per_joint(plan)
if (abs_rot_per_joint > max_rotation_per_joint).any():
return True
else:
return False
class pick_place():
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.ur5 = MoveGroupCommander("manipulator")
self.gripper = MoveGroupCommander("end_effector")
print("======================================================")
print(self.robot.get_group_names())
print(self.robot.get_link_names())
print(self.robot.get_joint_names())
print(self.robot.get_planning_frame())
print(self.ur5.get_end_effector_link())
print("======================================================")
self.ur5.set_max_velocity_scaling_factor(0.2)
self.ur5.set_max_acceleration_scaling_factor(0.2)
self.ur5.set_end_effector_link("fts_toolside")
self.ur5.set_planning_time(10.0)
#self.ur5.set_planner_id("RRTkConfigDefault")
self.gripper_ac = RobotiqActionClient('icl_phri_gripper/gripper_controller')
self.gripper_ac.wait_for_server()
self.gripper_ac.initiate()
self.gripper_ac.send_goal(0.08)
self.gripper_ac.wait_for_result()
self.sub = rospy.Subscriber('/target_position', Int32MultiArray, self.pickplace_cb)
def single_exuete(self, position, mode):
offset = 0.01
rospy.loginfo("let do a single exuete")
rospy.sleep(1)
position_copy = deepcopy(position)
position_copy += [0.14]
position_copy[1] = position_copy[1] + offset
pre_position = define_grasp([position_copy[0], position_copy[1], position_copy[2] + 0.2])
post_position = define_grasp([position_copy[0], position_copy[1], position_copy[2] + 0.2])
grasp_position = define_grasp(position_copy)
self.ur5.set_pose_target(pre_position)
rospy.loginfo("let's go to pre position")
self.ur5.go()
self.ur5.stop()
self.ur5.clear_pose_targets()
rospy.sleep(1)
self.ur5.set_pose_target(grasp_position)
rospy.loginfo("let's do this")
self.ur5.go()
self.ur5.stop()
self.ur5.clear_pose_targets()
if mode == "pick":
self.gripper_ac.send_goal(0)
if mode == "place":
self.gripper_ac.send_goal(0.08)
self.gripper_ac.wait_for_result()
rospy.loginfo("I got this")
rospy.sleep(1)
self.ur5.set_pose_target(post_position)
rospy.loginfo("move out")
self.ur5.go()
self.ur5.stop()
self.ur5.clear_pose_targets()
rospy.sleep(1)
def pair_exuete(self, pick_position, place_position):
rospy.loginfo("here we go pair")
if pick_position and place_position:
self.single_exuete(pick_position, "pick")
self.single_exuete(place_position, "place")
#rospy.sleep(1)
rospy.loginfo("let's go and get some rest")
rest_position = define_grasp([0.486, -0.152, 0.342])
self.ur5.set_pose_target(rest_position)
self.ur5.go()
self.ur5.stop()
self.ur5.clear_pose_targets()
rospy.sleep(1)
def pickplace_cb(self, msg):
#print(msg)
print(msg.data)
a = list(msg.data)
mean_x = np.mean([a[i] for i in range(0, len(a)-2, 2)])
mean_y = np.mean([a[i] for i in range(1, len(a)-2, 2)])
num_goals = (len(msg.data) -2)/2
rospy.loginfo("there is {} goals".format(num_goals))
for i in range(0, len(a)-2, 2):
pick_x, pick_y = coord_converter(msg.data[i], msg.data[i+1])
leeway_x = int(msg.data[i] - mean_x)
leeway_y = int(msg.data[i+1] - mean_y)
place_x, place_y = coord_converter(msg.data[-2] + leeway_x, msg.data[-1] + leeway_y)
print(pick_x, pick_y)
print(place_x, place_y)
self.pair_exuete([pick_x, pick_y], [place_x, place_y])
def __init__(self, handeye_client, mode):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
robot = moveit_commander.RobotCommander()
# 当运动规划失败后，允许重新规划
arm.allow_replanning(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.0001)
arm.set_goal_orientation_tolerance(0.0001)
if mode == "sim":
arm.set_max_velocity_scaling_factor(1.0)
elif mode == "real":
arm.set_max_velocity_scaling_factor(0.5)
arm.set_planning_time(0.05) # 规划时间限制
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
# print "[INFO] Current pose:", arm.get_current_pose(eef_link).pose
self.group = arm
self.robot = robot
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
# 控制机械臂运动到之前设置的姿态
# arm.set_named_target('calib1')
# arm.go()
# self.go_to_joint_state([2.5228, 0.8964, 0.9746, 0.8614, 2.7515, -1.8821, 0.1712])
self.mode = mode
self.listener = tf.TransformListener()
self.handeye_client = handeye_client
self.im_converter = image_converter()
self.aruco_pix_listener = aruco_pix_listener()
self.data_num = 0
if mode == "sim":
self.camera_link = "camera_link_optical"
self.states = [
[2.5230, 0.8961, 0.9750, 0.8609, 2.7510, -1.8819, 0.1710],
[2.4861, 0.9171, 0.9988, 0.8576, 2.8072, -1.8743, 0.1717],
[2.5231, 0.8961, 0.9738, 0.8604, 2.7517, -1.8818, 0.1700],
[2.4003, 0.7911, 0.9820, 1.0503, 2.8352, -1.6524, 0.3760],
[2.5049, 0.9006, 0.9405, 0.8542, 2.8436, -1.8724, 0.4127],
[2.6273, 0.8107, 0.9139, 0.9618, 2.7511, -1.7930, 0.5062],
[2.7162, 0.8069, 0.8263, 0.9200, 2.7147, -1.8470, 0.7433],
[2.7065, 0.8050, 0.8584, 0.9477, 2.4444, -1.8806, 0.1071],
[2.2901, 0.7806, 1.0289, 1.2211, 2.7996, -1.7092, 0.1444],
[2.1136, 0.7398, 1.0706, 1.2892, 2.7842, -1.5650, -0.0094],
[2.7180, 0.7731, 1.0472, 1.2663, 2.4937, -1.7672, 0.5462],
[2.7180, 0.7731, 1.0472, 1.2663, 2.4937, -1.7672, 0.5462],
[2.6515, 0.6861, 1.0594, 1.4848, 2.5607, -1.5177, 0.8066],
[2.7958, 0.8070, 0.9759, 1.2940, 2.4786, -1.6845, 0.7778],
[2.8772, 0.7243, 0.9153, 1.4943, 2.3478, -1.4724, 1.0759],
[2.3152, 0.8412, 1.1005, 1.1974, 2.5480, -1.6883, 0.2808],
[2.0790, 0.8522, 1.0670, 1.6003, 2.6245, -1.2628, 0.5669],
[2.1475, 1.0192, 1.0940, 1.3640, 2.6444, -1.3627, 0.3612],
[0.2200, -0.5008, -2.8969, 1.7277, 2.6600, -1.1654, 1.5059],
[0.4877, -0.7181, -0.2344, -1.3239, -0.1559, -1.4678, 0.5835],
[0.3773, -0.6314, -0.2302, -1.4444, -0.1600, -1.3351, 0.4067],
[-2.2371, 1.3616, 1.5669, -1.2129, 0.9923, -1.8344, -0.1351],
[-2.3273, 1.3885, 1.5713, -0.9358, 1.0748, -1.9300, 0.5635],
[-2.2465, 1.2267, 1.8483, -1.0450, 0.7416, -1.6672, 0.3275],
[-1.7879, 1.2150, 1.6576, -1.5896, 0.7221, -1.4276, 0.6940],
[-2.2530, 1.0939, 1.8129, -1.3149, 0.7196, -1.6466, 0.4342],
[-2.1126, 1.1457, 1.7960, -1.2610, 0.6560, -1.6326, 0.4547],
[-2.1270, 1.2311, 1.8561, -1.0124, 0.5739, -1.8181, 0.4290],
]
elif mode == "real":
self.camera_link = "camera_color_optical_frame"
self.states = [
[-0.1629, -0.1209, -0.0899, -0.6650, 0.1250, -1.9180, -1.7391],
[0.1768, -0.2480, -0.0637, -0.6867, -0.2229, -1.8912, -1.2967],
[0.1064, -0.0347, -0.0530, -1.1423, -0.0809, -1.5052, -1.6230],
[0.4235, -0.0861, -0.1177, -1.1818, -0.2675, -1.4303, -1.5148],
[0.4016, -0.0026, -0.0992, -1.2851, -0.3538, -1.4122, -1.5428],
[0.2777, -0.0294, -0.1911, -1.2395, -0.0141, -1.3563, -2.0564],
[2.9142, 0.1833, -2.7987, -1.0661, -0.0965, -1.4191, -1.8634],
[2.8310, -0.1007, -2.8406, -1.3918, 0.0263, -1.2798, -1.8718],
[2.6899, -0.0448, -2.9686, -1.3379, 0.2609, -1.3600, -2.1981],
[2.7554, -0.2997, -2.5511, -1.5645, -0.2672, -1.2493, -1.5705],
[2.7252, 0.2366, -2.6045, -0.9946, -0.2552, -1.5880, -1.2153],
[2.3309, 0.2733, -2.6384, -1.1497, 0.2391, -1.3979, -1.6642],
[2.1696, 0.5283, -2.6540, -0.8638, 0.3913, -1.5963, -1.8724],
[2.2318, 0.5134, -2.7120, -0.8413, 0.3381, -1.6116, -1.6351],
[2.3469, 0.5802, -2.7714, -0.6768, 0.2250, -1.7614, -1.4197],
[2.3452, 0.4840, -2.7232, -0.8959, 0.2437, -1.5798, -1.5464],
[2.3888, 0.3196, -2.7301, -1.1076, 0.1213, -1.4436, -1.5656],
[2.5365, 0.7079, -2.6854, -0.5944, 0.1202, -1.7089, -1.5256],
[2.3243, 0.5048, -2.5923, -1.0267, 0.1985, -1.3861, -1.6988],
[2.6772, 0.3100, -2.6433, -1.0222, -0.1752, -1.4295, -1.1358],
[2.5951, 0.3157, -2.5408, -1.0630, -0.2109, -1.3997, -1.1196],
[2.5800, 0.3364, -2.4705, -1.0478, -0.2944, -1.4289, -1.6568],
[2.7948, 0.3616, -2.5705, -1.0294, -0.4474, -1.5064, -0.9428],
[2.7595, 0.2951, -2.5461, -1.1454, -0.4809, -1.3790, -1.1017],
]
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.01)
# arm.set_goal_orientation_tolerance(0.1)
arm.set_max_velocity_scaling_factor(0.5)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂运动到之前设置的姿态
# arm.set_named_target('cali_2')
# arm.set_named_target('test_1')
arm.set_joint_value_target(
[0.055, 0.317, -0.033, -1.292, -0.099, -0.087, -1.575])
arm.go()
# exit()
pose = cal_end_pose_by_quat(arm.get_current_pose().pose, -0.2, 2)
arm.set_pose_target(pose)
arm.go()
# raw_input()
# 获取当前位姿数据最为机械臂运动的起始位姿
start_pose = arm.get_current_pose().pose
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
waypoints.append(start_pose)
# 按末端坐标系方向向量平移, 计算终点位姿
wpose = cal_end_pose_by_quat(start_pose, 0.35, 2)
waypoints.append(deepcopy(wpose))
def scale_trajectory_speed(traj, scale):
new_traj = RobotTrajectory()
new_traj.joint_trajectory = traj.joint_trajectory
n_joints = len(traj.joint_trajectory.joint_names)
n_points = len(traj.joint_trajectory.points)
points = list(traj.joint_trajectory.points)
for i in range(n_points):
point = JointTrajectoryPoint()
point.positions = traj.joint_trajectory.points[i].positions
point.time_from_start = traj.joint_trajectory.points[
i].time_from_start / scale
point.velocities = list(
traj.joint_trajectory.points[i].velocities)
point.accelerations = list(
traj.joint_trajectory.points[i].accelerations)
for j in range(n_joints):
point.velocities[j] = point.velocities[j] * scale
point.accelerations[
j] = point.accelerations[j] * scale * scale
points[i] = point
new_traj.joint_trajectory.points = points
return new_traj
def move_cartesian(waypoints, scale):
group = arm
# 开始笛卡尔空间轨迹规划
fraction = 0.0 # 路径规划覆盖率
maxtries = 10 # 最大尝试规划次数
attempts = 0 # 已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
group.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = group.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.")
plan = scale_trajectory_speed(plan, scale)
group.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + " success after " +
str(maxtries) + " attempts.")
move_cartesian(waypoints, 0.5)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
joint_target_4, joint_target_5, joint_target_init
]
joint_targets = []
for i in range(0, 361, 45):
for j in range(-20, 21, 20):
i_rads = i * math.pi / 180
j_rads = j * math.pi / 180
joint_target = rotate_joint(joint_target_init, i_rads,
JOINT_INDEX_BASE)
joint_target = rotate_joint(joint_target, j_rads,
JOINT_INDEX_UPPER_ARM)
joint_targets.append(joint_target)
for joint_target in joint_targets:
group.set_joint_value_target(joint_target)
group.set_max_velocity_scaling_factor(0.1)
plan = group.plan()
group.go(wait=True)
group.stop()
if rospy.is_shutdown():
break
rospy.sleep(1)
roscpp_shutdown()
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('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
print start_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.1
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)
import rospy
from moveit_commander import MoveGroupCommander
if __name__ == '__main__':
#init_node()
rospy.init_node('message', anonymous=True)
group = MoveGroupCommander("manipulator")
exec_vel = 0.5
while True:
rospy.loginfo("joint1 start")
group.set_max_velocity_scaling_factor(exec_vel)
group.set_joint_value_target([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
group.go()
rospy.loginfo("joint1 end")
rospy.loginfo("pose1 start")
group.set_max_velocity_scaling_factor(exec_vel)
group.set_pose_target([0.5, -0.2, 0.2, 0.0, 1.0, 0.0])
group.go()
rospy.loginfo("pose1 end")
rospy.loginfo("pose2 start")
group.set_max_velocity_scaling_factor(exec_vel)
group.set_pose_target([0.5, -0.2, 0.7, 0.0, 1.0, 0.0])
group.go()
rospy.loginfo("pose2 end")
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=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.2)
arm.set_goal_orientation_tolerance(0.1)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.1)
arm.set_max_velocity_scaling_factor(0.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
print(end_effector_link)
# 控制机械臂先回到初始化位置
# 获取当前位姿数据最为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
print(start_pose)
# 初始化路点列表
waypoints = []
os.system("rosrun pick_test send_gripper.py --value 0.0")
# 将初始位姿加入路点列表
# arm.set_named_target('test5')
# arm.go()
# rospy.sleep(1)
######
marker = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, getCarrot)
listener = tf.TransformListener()
r = rospy.Rate(10)
while not rospy.is_shutdown():
try:
(trans,
rot) = listener.lookupTransform('/base_link', '/ar_marker_4',
rospy.Time(0))
break
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
######
# 设置路点数据，并加入路点列表
wpose = deepcopy(start_pose)
print carrot
wpose.position.z -= carrot.z
waypoints.append(deepcopy(wpose))
# wpose.position.z -= carrot.z+0.1
# wpose.position.y += carrot.y
# 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)
if fraction < 1.0:
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
# 控制机械臂先回到初始化位置
waypoints = []
test_pose = arm.get_current_pose(end_effector_link).pose
wpose = deepcopy(test_pose)
wpose.position.x -= carrot.x + 0.03
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.02, # 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)
if fraction < 1.0:
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
########
os.system("rosrun pick_test send_gripper.py --value 0.8")
########
waypoints = []
pick_pose = arm.get_current_pose(end_effector_link).pose
wpose = deepcopy(pick_pose)
wpose.position.z += 0.1
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.02, # 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)
if fraction < 1.0:
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class MoveCup():
def __init__(self):
#basic initiatioon
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_tutorial')
self.robot = moveit_commander.RobotCommander()
################ Collision Object
self.scene = moveit_commander.PlanningSceneInterface()
table = CollisionObject()
primitive = SolidPrimitive()
primitive.type = primitive.BOX
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.stamp = rospy.Time.now()
box_pose.header.frame_id = 'tablelol'
box_pose.pose.position.x = 1.25
box_pose.pose.position.y = 0.0
box_pose.pose.position.z = -0.6
table.primitives.append(primitive)
table.primitive_poses.append(box_pose)
table.operation = table.ADD
self.scene.add_box('table', box_pose, size=(.077, .073, .122))
#use joint_group parameter to change which arm it uses?
self.joint_group = rospy.get_param('~arm', default="left_arm")
self.group = MoveGroupCommander(self.joint_group)
#self.group.set_planner_id("BKPIECEkConfigDefault")
#this node will scale any tf pose requests to be at most max_reach from the base frame
self.max_reach = rospy.get_param('~max_reach', default=1.1)
#define a start pose that we can move to before stuff runs
self.start_pose = PoseStamped()
self.start_pose = self.get_start_pose()
#remove this when working for realz
self.display_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=10)
self.rate = rospy.Rate(1)
self.ik_srv = rospy.ServiceProxy(
'ExternalTools/left/PositionKinematicsNode/IKService',
SolvePositionIK)
self.limb = baxter_interface.Limb('left')
self.rangesub = rospy.Subscriber('cup_center', geometry_msgs.msg.Point,
self.rangecb)
self.stop = False
self.planning = False
def rangecb(self, point):
if self.planning and point.z == 0:
rospy.loginfo('stop')
self.stop = True
self.move_start()
self.rangesub.unregister()
def callback(self, targetarray):
#callback that moves in a constrained path to anything published to /target_poses
##First, scale the position to be withing self.max_reach
#new_target = self.project_point(targetarray.data)
# rospy.loginfo(self.stop)
if not self.stop:
self.planning = True
new_target = self.project_point(targetarray)
target = PoseStamped()
target.header.stamp = rospy.Time.now()
target.header.frame_id = 'base'
target.pose.position = new_target
#change orientation to be upright
target.pose.orientation = self.start_pose.pose.orientation
#clear group info and set it again
self.group.clear_pose_targets()
# self.group.set_path_constraints(self.get_constraint())
self.group.set_planning_time(10)
# self.group.set_pose_target(target)
################### Try joint space planning
jt_state = JointState()
jt_state.header.stamp = rospy.Time.now()
angles = self.limb.joint_angles()
jt_state.name = list(angles.keys())
jt_state.position = list(angles.values())
jt_state.header.frame_id = 'base'
result = self.ik_srv([target], [jt_state], 0)
angles = {}
i = 0
for name in result.joints[0].name:
angles[name] = result.joints[0].position[i]
i = i + 1
self.group.set_joint_value_target(angles)
#plan and execute plan. If I find a way, I should add error checking her
#currently, if the plan fails, it just doesn't move and waits for another pose to be published
plan = self.group.plan()
self.group.execute(plan)
self.rate.sleep()
return
def scale_movegroup(self, vel=.9, acc=.9):
#slows down baxters arm so we stop getting all those velocity limit errors
self.group.set_max_velocity_scaling_factor(vel)
self.group.set_max_acceleration_scaling_factor(acc)
def unscale_movegroup(self):
self.group.set_max_velocity_scaling_factor(1)
self.group.set_max_acceleration_scaling_factor(1)
def start_baxter_interface(self):
#I copied this from an example but have no idea what it does
self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate',
UInt16,
queue_size=10)
self._left_arm = baxter_interface.limb.Limb("left")
self._left_joint_names = self._left_arm.joint_names()
print(self._left_arm.endpoint_pose())
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
# set joint state publishing to 100Hz
self._pub_rate.publish(100)
return
def get_start_pose(self, point=[.9, 0.2, 0], rpy=[0, math.pi / 2, 0]):
#define a starting position for the move_start method
new_p = PoseStamped()
new_p.header.frame_id = self.robot.get_planning_frame()
new_p.pose.position.x = point[0]
new_p.pose.position.y = point[1]
new_p.pose.position.z = point[2]
p_orient = tf.transformations.quaternion_from_euler(
rpy[0], rpy[1], rpy[2])
p_orient = Quaternion(*p_orient)
new_p.pose.orientation = p_orient
return (new_p)
def project_point(self, multiarray):
#scales an array and returns a point (see: Pose.position) to be within self.max_reach
#convert points from sonar ring frame to shoulder frame
x = multiarray.data[2] + sr[0] - lls[0]
y = multiarray.data[0] + sr[1] - lls[1]
z = (-1 * multiarray.data[1]) + sr[2] - lls[2]
#scale point to a finite reach distance from the shoulder
obj_dist = math.sqrt(x**2 + y**2 + z**2)
scale_val = min(self.max_reach / obj_dist, .99)
point_scaled = Point()
#scale point and bring into the base frames
point_scaled.x = scale_val * x + lls[0]
point_scaled.y = scale_val * y + lls[1]
point_scaled.z = scale_val * z + lls[2]
return (point_scaled)
def move_random(self):
#moves baxter to a random position. used for testing
randstate = PoseStamped()
randstate = self.group.get_random_pose()
self.group.clear_pose_targets()
self.group.set_pose_target(randstate)
self.group.set_planning_time(8)
self.scale_movegroup()
plan = self.group.plan()
while len(
plan.joint_trajectory.points) == 1 and not rospy.is_shutdown():
print('plan no work')
plan = self.group.plan()
self.group.execute(plan)
self.rate.sleep()
return
def move_random_constrained(self):
#move baxter to a random position with constrained path planning. also for testing
self.scale_movegroup()
randstate = PoseStamped()
randstate = self.group.get_random_pose()
self.group.clear_pose_targets()
self.group.set_pose_target(randstate)
self.group.set_path_constraints(self.get_constraint())
self.group.set_planning_time(100)
self.scale_movegroup()
constrained_plan = self.group.plan()
self.group.execute(constrained_plan)
self.unscale_movegroup()
rospy.sleep(3)
return
def move_start(self):
#move baxter to the self.start_pose pose
self.group.clear_pose_targets()
self.group.set_pose_target(self.start_pose)
self.group.set_planning_time(10)
print('moving to start')
plan = self.group.plan()
self.group.execute(plan)
print('at start')
self.rate.sleep()
return
def get_constraint(self,
euler_orientation=[0, math.pi / 2, 0],
tol=[3, 3, .5]):
#method takes euler-angle inputs, this converts it to a quaternion
q_orientation = tf.transformations.quaternion_from_euler(
euler_orientation[0], euler_orientation[1], euler_orientation[2])
orientation_msg = Quaternion(q_orientation[0], q_orientation[1],
q_orientation[2], q_orientation[3])
#defines a constraint that sets the end-effector so a cup in it's hand will be upright, or straight upside-down
constraint = Constraints()
constraint.name = 'upright_wrist'
upright_orientation = OrientationConstraint()
upright_orientation.link_name = self.group.get_end_effector_link()
upright_orientation.orientation = orientation_msg
upright_orientation.absolute_x_axis_tolerance = tol[0]
upright_orientation.absolute_y_axis_tolerance = tol[1]
upright_orientation.absolute_z_axis_tolerance = tol[2]
upright_orientation.weight = 1.0
upright_orientation.header = self.start_pose.header
constraint.orientation_constraints.append(upright_orientation)
return (constraint)
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
group_name = "arm"
arm = MoveGroupCommander(group_name)
robot = moveit_commander.RobotCommander()
# Get joint bounds
joint_names = robot.get_joint_names(group=group_name)
joint_bounds = []
for joint_name in joint_names:
joint = robot.get_joint(joint_name)
joint_bounds.append(joint.bounds())
print("[INFO] " + joint_name + "_bounds:", joint.bounds())
# 当运动规划失败后，允许重新规划
arm.allow_replanning(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.01)
arm.set_max_velocity_scaling_factor(0.5)
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_planning_time(0.08) # 规划时间限制为2秒
# arm.set_num_planning_attempts(1) # 规划1次
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
scene = moveit_commander.PlanningSceneInterface()
scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
print("[INFO] Current pose:\n", arm.get_current_pose().pose)
self.scene = scene
self.scene_pub = scene_pub
self.colors = dict()
self.group = arm
self.robot = robot
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.joint_bounds = joint_bounds
self.broadcaster = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
self.gripper_len = 0.095 # 手爪实际长度0.165m, 虚拟夹爪深度0.075m 0.16-0.065=0.095m
self.approach_distance = 0.06
self.back_distance = 0.05
class SrRobotCommander(object):
"""
Base class for hand and arm commanders
"""
def __init__(self, name):
"""
Initialize MoveGroupCommander object
@param name - name of the MoveIt group
"""
self._name = name
self._move_group_commander = MoveGroupCommander(name)
self._robot_commander = RobotCommander()
self._robot_name = self._robot_commander._r.get_robot_name()
self.refresh_named_targets()
self._warehouse_name_get_srv = rospy.ServiceProxy(
"get_robot_state", GetState)
self._planning_scene = PlanningSceneInterface()
self._joint_states_lock = threading.Lock()
self._joint_states_listener = \
rospy.Subscriber("joint_states", JointState,
self._joint_states_callback, queue_size=1)
self._joints_position = {}
self._joints_velocity = {}
self._joints_effort = {}
self._joints_state = None
self._clients = {}
self.__plan = None
self._controllers = {}
rospy.wait_for_service('compute_ik')
self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK)
controller_list_param = rospy.get_param("/move_group/controller_list")
# create dictionary with name of controllers and corresponding joints
self._controllers = {
item["name"]: item["joints"]
for item in controller_list_param
}
self._set_up_action_client(self._controllers)
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer)
threading.Thread(None, rospy.spin)
def set_planner_id(self, planner_id):
self._move_group_commander.set_planner_id(planner_id)
def set_num_planning_attempts(self, num_planning_attempts):
self._move_group_commander.set_num_planning_attempts(
num_planning_attempts)
def set_planning_time(self, seconds):
self._move_group_commander.set_planning_time(seconds)
def get_end_effector_pose_from_named_state(self, name):
state = self._warehouse_name_get_srv(name, self._robot_name).state
return self.get_end_effector_pose_from_state(state)
def get_end_effector_pose_from_state(self, state):
header = Header()
fk_link_names = [self._move_group_commander.get_end_effector_link()]
header.frame_id = self._move_group_commander.get_pose_reference_frame()
response = self._forward_k(header, fk_link_names, state)
return response.pose_stamped[0]
def get_planning_frame(self):
return self._move_group_commander.get_planning_frame()
def set_pose_reference_frame(self, reference_frame):
self._move_group_commander.set_pose_reference_frame(reference_frame)
def get_group_name(self):
return self._name
def refresh_named_targets(self):
self._srdf_names = self.__get_srdf_names()
self._warehouse_names = self.__get_warehouse_names()
def set_max_velocity_scaling_factor(self, value):
self._move_group_commander.set_max_velocity_scaling_factor(value)
def set_max_acceleration_scaling_factor(self, value):
self._move_group_commander.set_max_acceleration_scaling_factor(value)
def allow_looking(self, value):
self._move_group_commander.allow_looking(value)
def allow_replanning(self, value):
self._move_group_commander.allow_replanning(value)
def execute(self):
"""
Executes the last plan made.
"""
if self.check_plan_is_valid():
self._move_group_commander.execute(self.__plan)
self.__plan = None
else:
rospy.logwarn("No plans were made, not executing anything.")
def execute_plan(self, plan):
if self.check_given_plan_is_valid(plan):
self._move_group_commander.execute(plan)
self.__plan = None
else:
rospy.logwarn("Plan is not valid, not executing anything.")
def move_to_joint_value_target(self,
joint_states,
wait=True,
angle_degrees=False):
"""
Set target of the robot's links and moves to it.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param wait - should method wait for movement end or not
@param angle_degrees - are joint_states in degrees or not
"""
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_joint_value_target(joint_states_cpy)
self._move_group_commander.go(wait=wait)
def plan_to_joint_value_target(self, joint_states, angle_degrees=False):
"""
Set target of the robot's links and plans.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param angle_degrees - are joint_states in degrees or not
This is a blocking method.
"""
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_joint_value_target(joint_states_cpy)
self.__plan = self._move_group_commander.plan()
return self.__plan
def check_plan_is_valid(self):
"""
Checks if current plan contains a valid trajectory
"""
return (self.__plan is not None
and len(self.__plan.joint_trajectory.points) > 0)
def check_given_plan_is_valid(self, plan):
"""
Checks if given plan contains a valid trajectory
"""
return (plan is not None and len(plan.joint_trajectory.points) > 0)
def get_robot_name(self):
return self._robot_name
def named_target_in_srdf(self, name):
return name in self._srdf_names
def set_named_target(self, name):
if name in self._srdf_names:
self._move_group_commander.set_named_target(name)
elif (name in self._warehouse_names):
response = self._warehouse_name_get_srv(name, self._robot_name)
active_names = self._move_group_commander._g.get_active_joints()
joints = response.state.joint_state.name
positions = response.state.joint_state.position
js = {}
for n, this_name in enumerate(joints):
if this_name in active_names:
js[this_name] = positions[n]
self._move_group_commander.set_joint_value_target(js)
else:
rospy.logerr("Unknown named state '%s'..." % name)
return False
return True
def get_named_target_joint_values(self, name):
output = dict()
if (name in self._srdf_names):
output = self._move_group_commander.\
_g.get_named_target_values(str(name))
elif (name in self._warehouse_names):
js = self._warehouse_name_get_srv(
name, self._robot_name).state.joint_state
for x, n in enumerate(js.name):
if n in self._move_group_commander._g.get_joints():
output[n] = js.position[x]
else:
rospy.logerr("No target named %s" % name)
return None
return output
def get_end_effector_link(self):
return self._move_group_commander.get_end_effector_link()
def get_current_pose(self, reference_frame=None):
"""
Get the current pose of the end effector.
@param reference_frame - The desired reference frame in which end effector pose should be returned.
If none is passed, it will use the planning frame as reference.
@return geometry_msgs.msg.Pose() - current pose of the end effector
"""
if reference_frame is not None:
try:
trans = self.tf_buffer.lookup_transform(
reference_frame,
self._move_group_commander.get_end_effector_link(),
rospy.Time(0), rospy.Duration(5.0))
current_pose = geometry_msgs.msg.Pose()
current_pose.position.x = trans.transform.translation.x
current_pose.position.y = trans.transform.translation.y
current_pose.position.z = trans.transform.translation.z
current_pose.orientation.x = trans.transform.rotation.x
current_pose.orientation.y = trans.transform.rotation.y
current_pose.orientation.z = trans.transform.rotation.z
current_pose.orientation.w = trans.transform.rotation.w
return current_pose
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rospy.logwarn(
"Couldn't get the pose from " +
self._move_group_commander.get_end_effector_link() +
" in " + reference_frame + " reference frame")
return None
else:
return self._move_group_commander.get_current_pose().pose
def get_current_state(self):
"""
Get the current joint state of the group being used.
@return a dictionary with the joint names as keys and current joint values
"""
joint_names = self._move_group_commander._g.get_active_joints()
joint_values = self._move_group_commander._g.get_current_joint_values()
return dict(zip(joint_names, joint_values))
def get_current_state_bounded(self):
"""
Get the current joint state of the group being used, enforcing that they are within each joint limits.
@return a dictionary with the joint names as keys and current joint values
"""
current = self._move_group_commander._g.get_current_state_bounded()
names = self._move_group_commander._g.get_active_joints()
output = {n: current[n] for n in names if n in current}
return output
def get_robot_state_bounded(self):
return self._move_group_commander._g.get_current_state_bounded()
def move_to_named_target(self, name, wait=True):
"""
Set target of the robot's links and moves to it
@param name - name of the target pose defined in SRDF
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
if self.set_named_target(name):
self._move_group_commander.go(wait=wait)
def plan_to_named_target(self, name):
"""
Set target of the robot's links and plans
This is a blocking method.
@param name - name of the target pose defined in SRDF
"""
self._move_group_commander.set_start_state_to_current_state()
if self.set_named_target(name):
self.__plan = self._move_group_commander.plan()
def __get_warehouse_names(self):
try:
list_srv = rospy.ServiceProxy("list_robot_states", ListStates)
return list_srv("", self._robot_name).states
except rospy.ServiceException as exc:
rospy.logwarn("Couldn't access warehouse: " + str(exc))
return list()
def _reset_plan(self):
self.__plan = None
def _set_plan(self, plan):
self.__plan = plan
def __get_srdf_names(self):
return self._move_group_commander._g.get_named_targets()
def get_named_targets(self):
"""
Get the complete list of named targets, from SRDF
as well as warehouse poses if available.
@return list of strings containing names of targets.
"""
return self._srdf_names + self._warehouse_names
def get_joints_position(self):
"""
Returns joints position
@return - dictionary with joints positions
"""
with self._joint_states_lock:
return self._joints_position
def get_joints_velocity(self):
"""
Returns joints velocities
@return - dictionary with joints velocities
"""
with self._joint_states_lock:
return self._joints_velocity
def _get_joints_effort(self):
"""
Returns joints effort
@return - dictionary with joints efforts
"""
with self._joint_states_lock:
return self._joints_effort
def get_joints_state(self):
"""
Returns joints state
@return - JointState message
"""
with self._joint_states_lock:
return self._joints_state
def run_joint_trajectory(self, joint_trajectory):
"""
Moves robot through all joint states with specified timeouts
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
"""
plan = RobotTrajectory()
plan.joint_trajectory = joint_trajectory
self._move_group_commander.execute(plan)
def make_named_trajectory(self, trajectory):
"""
Makes joint value trajectory from specified by named poses (either from
SRDF or from warehouse)
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements (n.b either name or joint_angles is required)
- name -> the name of the way point
- joint_angles -> a dict of joint names and angles
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
- degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent.
"""
current = self.get_current_state_bounded()
joint_trajectory = JointTrajectory()
joint_names = current.keys()
joint_trajectory.joint_names = joint_names
start = JointTrajectoryPoint()
start.positions = current.values()
start.time_from_start = rospy.Duration.from_sec(0.001)
joint_trajectory.points.append(start)
time_from_start = 0.0
for wp in trajectory:
joint_positions = None
if 'name' in wp.keys():
joint_positions = self.get_named_target_joint_values(
wp['name'])
elif 'joint_angles' in wp.keys():
joint_positions = copy.deepcopy(wp['joint_angles'])
if 'degrees' in wp.keys() and wp['degrees']:
for joint, angle in joint_positions.iteritems():
joint_positions[joint] = radians(angle)
if joint_positions is None:
rospy.logerr(
"Invalid waypoint. Must contain valid name for named target or dict of joint angles."
)
return None
new_positions = {}
for n in joint_names:
new_positions[n] = joint_positions[
n] if n in joint_positions else current[n]
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = [
new_positions[n] for n in joint_names
]
current = new_positions
time_from_start += wp['interpolate_time']
trajectory_point.time_from_start = rospy.Duration.from_sec(
time_from_start)
joint_trajectory.points.append(trajectory_point)
if 'pause_time' in wp and wp['pause_time'] > 0:
extra = JointTrajectoryPoint()
extra.positions = trajectory_point.positions
time_from_start += wp['pause_time']
extra.time_from_start = rospy.Duration.from_sec(
time_from_start)
joint_trajectory.points.append(extra)
return joint_trajectory
def send_stop_trajectory_unsafe(self):
"""
Sends a trajectory of all active joints at their current position.
This stops the robot.
"""
current = self.get_current_state_bounded()
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = current.values()
trajectory_point.time_from_start = rospy.Duration.from_sec(0.1)
trajectory = JointTrajectory()
trajectory.points.append(trajectory_point)
trajectory.joint_names = current.keys()
self.run_joint_trajectory_unsafe(trajectory)
def run_named_trajectory_unsafe(self, trajectory, wait=False):
"""
Moves robot through trajectory specified by named poses, either from
SRDF or from warehouse. Runs trajectory directly via contoller.
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements:
- name -> the name of the way point
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
"""
joint_trajectory = self.make_named_trajectory(trajectory)
if joint_trajectory is not None:
self.run_joint_trajectory_unsafe(joint_trajectory, wait)
def run_named_trajectory(self, trajectory):
"""
Moves robot through trajectory specified by named poses, either from
SRDF or from warehouse. Runs trajectory via moveit.
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements:
- name -> the name of the way point
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
"""
joint_trajectory = self.make_named_trajectory(trajectory)
if joint_trajectory is not None:
self.run_joint_trajectory(joint_trajectory)
def move_to_position_target(self, xyz, end_effector_link="", wait=True):
"""
Specify a target position for the end-effector and moves to it
@param xyz - new position of end-effector
@param end_effector_link - name of the end effector link
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_position_target(xyz, end_effector_link)
self._move_group_commander.go(wait=wait)
def plan_to_position_target(self, xyz, end_effector_link=""):
"""
Specify a target position for the end-effector and plans.
This is a blocking method.
@param xyz - new position of end-effector
@param end_effector_link - name of the end effector link
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_position_target(xyz, end_effector_link)
self.__plan = self._move_group_commander.plan()
def move_to_pose_target(self, pose, end_effector_link="", wait=True):
"""
Specify a target pose for the end-effector and moves to it
@param pose - new pose of end-effector: a Pose message, a PoseStamped
message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
of 7 floats [x, y, z, qx, qy, qz, qw]
@param end_effector_link - name of the end effector link
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_pose_target(pose, end_effector_link)
self._move_group_commander.go(wait=wait)
def plan_to_pose_target(self,
pose,
end_effector_link="",
alternative_method=False):
"""
Specify a target pose for the end-effector and plans.
This is a blocking method.
@param pose - new pose of end-effector: a Pose message, a PoseStamped
message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
of 7 floats [x, y, z, qx, qy, qz, qw]
@param end_effector_link - name of the end effector link
@param alternative_method - use set_joint_value_target instead of set_pose_target
"""
self._move_group_commander.set_start_state_to_current_state()
if alternative_method:
self._move_group_commander.set_joint_value_target(
pose, end_effector_link)
else:
self._move_group_commander.set_pose_target(pose, end_effector_link)
self.__plan = self._move_group_commander.plan()
return self.__plan
def _joint_states_callback(self, joint_state):
"""
The callback function for the topic joint_states.
It will store the received joint position, velocity and efforts
information into dictionaries
@param joint_state - the message containing the joints data.
"""
with self._joint_states_lock:
self._joints_state = joint_state
self._joints_position = {
n: p
for n, p in zip(joint_state.name, joint_state.position)
}
self._joints_velocity = {
n: v
for n, v in zip(joint_state.name, joint_state.velocity)
}
self._joints_effort = {
n: v
for n, v in zip(joint_state.name, joint_state.effort)
}
def _set_up_action_client(self, controller_list):
"""
Sets up an action client to communicate with the trajectory controller
"""
self._action_running = {}
for controller_name in controller_list.keys():
self._action_running[controller_name] = False
service_name = controller_name + "/follow_joint_trajectory"
self._clients[controller_name] = SimpleActionClient(
service_name, FollowJointTrajectoryAction)
if self._clients[controller_name].wait_for_server(
timeout=rospy.Duration(4)) is False:
err_msg = 'Failed to connect to action server ({}) in 4 sec'.format(
service_name)
rospy.logwarn(err_msg)
def move_to_joint_value_target_unsafe(self,
joint_states,
time=0.002,
wait=True,
angle_degrees=False):
"""
Set target of the robot's links and moves to it.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param time - time in s (counting from now) for the robot to reach the
target (it needs to be greater than 0.0 for it not to be rejected by
the trajectory controller)
@param wait - should method wait for movement end or not
@param angle_degrees - are joint_states in degrees or not
"""
# self._update_default_trajectory()
# self._set_targets_to_default_trajectory(joint_states)
goals = {}
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
for controller in self._controllers:
controller_joints = self._controllers[controller]
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = []
point = JointTrajectoryPoint()
point.positions = []
for x in joint_states_cpy.keys():
if x in controller_joints:
goal.trajectory.joint_names.append(x)
point.positions.append(joint_states_cpy[x])
point.time_from_start = rospy.Duration.from_sec(time)
goal.trajectory.points = [point]
goals[controller] = goal
self._call_action(goals)
if not wait:
return
for i in self._clients.keys():
if not self._clients[i].wait_for_result():
rospy.loginfo("Trajectory not completed")
def action_is_running(self, controller=None):
if controller is not None:
return self._action_running[controller]
for controller_running in self._action_running.values():
if controller_running:
return True
return False
def _action_done_cb(self, controller, terminal_state, result):
self._action_running[controller] = False
def _call_action(self, goals):
for client in self._clients:
self._action_running[client] = True
self._clients[client].send_goal(
goals[client],
lambda terminal_state, result: self._action_done_cb(
client, terminal_state, result))
def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True):
"""
Moves robot through all joint states with specified timeouts
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
@param wait - should method wait for movement end or not
"""
goals = {}
for controller in self._controllers:
controller_joints = self._controllers[controller]
goal = FollowJointTrajectoryGoal()
goal.trajectory = copy.deepcopy(joint_trajectory)
indices_of_joints_in_this_controller = []
for i, joint in enumerate(joint_trajectory.joint_names):
if joint in controller_joints:
indices_of_joints_in_this_controller.append(i)
goal.trajectory.joint_names = [
joint_trajectory.joint_names[i]
for i in indices_of_joints_in_this_controller
]
for point in goal.trajectory.points:
if point.positions:
point.positions = [
point.positions[i]
for i in indices_of_joints_in_this_controller
]
if point.velocities:
point.velocities = [
point.velocities[i]
for i in indices_of_joints_in_this_controller
]
if point.effort:
point.effort = [
point.effort[i]
for i in indices_of_joints_in_this_controller
]
goals[controller] = goal
self._call_action(goals)
if not wait:
return
for i in self._clients.keys():
if not self._clients[i].wait_for_result():
rospy.loginfo("Trajectory not completed")
def plan_to_waypoints_target(self,
waypoints,
reference_frame=None,
eef_step=0.005,
jump_threshold=0.0):
"""
Specify a set of waypoints for the end-effector and plans.
This is a blocking method.
@param reference_frame - the reference frame in which the waypoints are given
@param waypoints - an array of poses of end-effector
@param eef_step - configurations are computed for every eef_step meters
@param jump_threshold - maximum distance in configuration space between consecutive points in the resulting path
"""
old_frame = self._move_group_commander.get_pose_reference_frame()
if reference_frame is not None:
self.set_pose_reference_frame(reference_frame)
(self.__plan,
fraction) = self._move_group_commander.compute_cartesian_path(
waypoints, eef_step, jump_threshold)
self.set_pose_reference_frame(old_frame)
def set_teach_mode(self, teach):
"""
Activates/deactivates the teach mode for the robot.
Activation: stops the the trajectory controllers for the robot, and
sets it to teach mode.
Deactivation: stops the teach mode and starts trajectory controllers
for the robot.
Currently this method blocks for a few seconds when called on a hand,
while the hand parameters are reloaded.
@param teach - bool to activate or deactivate teach mode
"""
if teach:
mode = RobotTeachModeRequest.TEACH_MODE
else:
mode = RobotTeachModeRequest.TRAJECTORY_MODE
self.change_teach_mode(mode, self._name)
def move_to_trajectory_start(self, trajectory, wait=True):
"""
Make and execute a plan from the current state to the first state in an pre-existing trajectory
@param trajectory - moveit_msgs/JointTrajectory
@param wait - Bool to specify if movement should block untill finished.
"""
if len(trajectory.points) <= 0:
rospy.logerr("Trajectory has no points in it, can't reverse...")
return None
first_point = trajectory.points[0]
end_state = dict(zip(trajectory.joint_names, first_point.positions))
self.move_to_joint_value_target(end_state, wait=wait)
@staticmethod
def change_teach_mode(mode, robot):
teach_mode_client = rospy.ServiceProxy('/teach_mode', RobotTeachMode)
req = RobotTeachModeRequest()
req.teach_mode = mode
req.robot = robot
try:
resp = teach_mode_client(req)
if resp.result == RobotTeachModeResponse.ERROR:
rospy.logerr("Failed to change robot %s to mode %d", robot,
mode)
else:
rospy.loginfo("Changed robot %s to mode %d Result = %d", robot,
mode, resp.result)
except rospy.ServiceException:
rospy.logerr("Failed to call service teach_mode")
def get_ik(self, target_pose, avoid_collisions=False, joint_states=None):
"""
Computes the inverse kinematics for a given pose. It returns a JointState
@param target_pose - A given pose of type PoseStamped
@param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false
"""
service_request = PositionIKRequest()
service_request.group_name = self._name
service_request.ik_link_name = self._move_group_commander.get_end_effector_link(
)
service_request.pose_stamped = target_pose
service_request.timeout.secs = 0.5
service_request.avoid_collisions = avoid_collisions
if joint_states is None:
service_request.robot_state.joint_state = self.get_joints_state()
else:
service_request.robot_state.joint_state = joint_states
try:
resp = self._compute_ik(ik_request=service_request)
# Check if error_code.val is SUCCESS=1
if resp.error_code.val != 1:
if resp.error_code.val == -10:
rospy.logerr("Unreachable point: Start state in collision")
elif resp.error_code.val == -12:
rospy.logerr("Unreachable point: Goal state in collision")
elif resp.error_code.val == -31:
rospy.logerr("Unreachable point: No IK solution")
else:
rospy.logerr("Unreachable point (error: %s)" %
resp.error_code)
return
else:
return resp.solution.joint_state
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('oval_trajectory_planning', 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.3)
arm.set_max_velocity_scaling_factor(0.5)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂先回到初始化位置
arm.set_named_target('start')
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.38665
target_pose.pose.position.y = 0
target_pose.pose.position.z = 0.37226
target_pose.pose.orientation.x = 1
target_pose.pose.orientation.y = 0.00007
target_pose.pose.orientation.z = -0.0024684
target_pose.pose.orientation.w = 0.00003
# 设置机械臂终端运动的目标位姿
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
long_axis = 0.03
short_axis=0.06
for th in numpy.arange(0, 6.28, 0.005):
target_pose.pose.position.y = centerA + long_axis * math.cos(th)-0.03
target_pose.pose.position.z = centerB + short_axis * math.sin(th)
wpose = deepcopy(target_pose.pose)
waypoints.append(deepcopy(wpose))
#print('%f, %f' % (Y, Z))
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)
rospy.loginfo("位姿控制---->椭圆轨迹规划 成功!")
# 控制机械臂先回到初始化位置
arm.set_named_target('start')
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('TCP_Move', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('xarm6')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('link_base')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.1)
arm.set_max_velocity_scaling_factor(0.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 角度弧度转换
j1 = 90.0 / 180 * math.pi
j2 = -18.6 / 180 * math.pi
j3 = -28.1 / 180 * math.pi
j4 = 1.0 / 180 * math.pi
j5 = 47.6 / 180 * math.pi
j6 = -0.9 / 180 * math.pi
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [j1, j2, j3, j4, j5, j6]
arm.set_joint_value_target(joint_positions)
arm.go()
rospy.sleep(1)
# 向下按压门把手
current_pose = arm.get_current_joint_values()
current_pose[4] += (20.0 / 180.0) * math.pi
arm.set_joint_value_target(current_pose)
arm.go()
rospy.sleep(1)
#推开门
current_pose = arm.get_current_joint_values()
current_pose[0] -= (42.0 / 180.0) * math.pi
current_pose[1] += (2.0 / 180.0) * math.pi
current_pose[2] -= (11.4 / 180.0) * math.pi
arm.set_joint_value_target(current_pose)
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)
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('manipulator')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(1)
arm.set_max_velocity_scaling_factor(1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
# joint_positions = [-1.1825355285207154e-05, -0.5670001723145425, 0.9280267992893177, 5.3613237943855825e-06, -1.9227286805556647, -6.314999961870072e-05[-1.1825355285207154e-05, -0.5670001723145425, 0.9280267992893177, 5.3613237943855825e-06, -1.9227286805556647, -6.314999961870072e-05]
# arm.set_joint_value_target(joint_positions)
# # 控制机械臂完成运动
# arm.go()
listener = tf.TransformListener()
while not rospy.is_shutdown():
try:
# 获取源坐标系base_link与目标坐标系iron_block之间最新的一次坐标转换
(trans,
rot) = listener.lookupTransform('/base_link', '/iron_block',
rospy.Time(0))
object_detect_x = trans[0]
object_detect_y = trans[1]
# object_detect_z = trans[2]
# if(object_detect_x>0.033 or object_detect_x<0.027):
# # print("detect x error!")
# continue
if (object_detect_y > 0.25 or object_detect_y < 0.2):
# print("detect y error!")
continue
# if(trans[2]>-0.04 or trans[2]<-0.08):
# print("detect z error!")
# continue
print(trans)
# print(rot)
# 设置机械臂工作空间中的目标位姿，位置使用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 = object_detect_x
target_pose.pose.position.y = object_detect_y - 0.05
target_pose.pose.position.z = 0.15
target_pose.pose.orientation.x = -rot[0]
target_pose.pose.orientation.y = -rot[1]
target_pose.pose.orientation.z = -rot[2]
target_pose.pose.orientation.w = -rot[3]
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 设置机械臂终端运动的目标位姿
arm.set_pose_target(target_pose, end_effector_link)
# 规划运动路径
traj = arm.plan()
num_of_points = len(traj.joint_trajectory.points)
d = rospy.Duration.from_sec(1)
print traj.joint_trajectory.points[num_of_points -
1].time_from_start
for index in range(num_of_points):
traj.joint_trajectory.points[index].time_from_start *= \
d/traj.joint_trajectory.points[num_of_points-1].time_from_start
# 按照规划的运动路径控制机械臂运动
arm.execute(traj)
# # 设置机械臂工作空间中的目标位姿，位置使用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 = object_detect_x
# target_pose.pose.position.y = object_detect_y-0.1
# target_pose.pose.position.z = 0.10
# target_pose.pose.orientation.x = -rot[0]
# target_pose.pose.orientation.y = -rot[1]
# target_pose.pose.orientation.z = -rot[2]
# target_pose.pose.orientation.w = -rot[3]
# # 设置机器臂当前的状态作为运动初始状态
# arm.set_start_state_to_current_state()
# # 设置机械臂终端运动的目标位姿
# arm.set_pose_target(target_pose, end_effector_link)
# # 规划运动路径
# traj = arm.plan()
# num_of_points = len(traj.joint_trajectory.points)
# d = rospy.Duration.from_sec(0.2)
# print traj.joint_trajectory.points[num_of_points-1].time_from_start
# for index in range(num_of_points):
# traj.joint_trajectory.points[index].time_from_start *= \
# d/traj.joint_trajectory.points[num_of_points-1].time_from_start
# # 按照规划的运动路径控制机械臂运动
# arm.execute(traj)
# 获取当前位姿数据最为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
wpose.position.y -= 0.3
waypoints.append(deepcopy(wpose))
fraction = 0.0 #路径规划覆盖率
maxtries = 10 #最大尝试规划次数
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.1, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# print plan
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo(
"Path computed successfully. Moving the arm.")
# num_of_points = len(plan.joint_trajectory.points)
# d = rospy.Duration.from_sec(6)
# scale_factor = d/plan.joint_trajectory.points[num_of_points-1].time_from_start
# print scale_factor
# for index in range(num_of_points):
# plan.joint_trajectory.points[index].time_from_start *= scale_factor
# print plan
# Scale the trajectory speed by a factor of 0.25
new_traj = self.scale_trajectory_speed(plan, 0.25)
arm.execute(new_traj)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + " success after " +
str(maxtries) + " attempts.")
# #rospy.sleep(1)
# # os.system("python ~/MQTT/ros_gripper_control.py 1100")
# # rospy.sleep(1)
# 回起始点
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
# rospy.loginfo("go origin position")
# joint_positions = [-1.1825355285207154e-05, -0.5670001723145425, 0.9280267992893177, 5.3613237943855825e-06, -1.9227286805556647, -6.314999961870072e-05[-1.1825355285207154e-05, -0.5670001723145425, 0.9280267992893177, 5.3613237943855825e-06, -1.9227286805556647, -6.314999961870072e-05]
# arm.set_joint_value_target(joint_positions)
# # 控制机械臂完成运动
# arm.go()
# # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
# joint_positions = [-0.8188115183246074, -0.43414310645724263, -0.2707591623036649, -0.008221640488656196, -0.8719249563699826, -0.8136649214659686]
# arm.set_joint_value_target(joint_positions)
# # 控制机械臂完成运动
# arm.go()
# os.system("python ~/MQTT/ros_gripper_control.py 0")
# rospy.sleep(1)
# #回起始点
# joint_positions = [-3.490401396160559e-05, -0.6098499127399651, 1.0162722513089006, 8.37696335078534e-05, -1.959607329842932, 2.268760907504363e-05]
# arm.set_joint_value_target(joint_positions)
# # 控制机械臂完成运动
# arm.go()
rospy.sleep(2)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
# listener = tf.TransformListener()
# while not rospy.is_shutdown():
# try:
# # 获取源坐标系base_link与目标坐标系iron_block之间最新的一次坐标转换
# (trans,rot) = listener.lookupTransform('/camera_color_optical_frame', '/iron_block', rospy.Time(0))
# object_detect_x = trans[0]
# object_detect_y = trans[1]
# if(object_detect_x>0.033 or object_detect_x<0.027):
# # print("detect x error!")
# continue
# if(object_detect_y>0.040 or object_detect_y<0.020):
# # print("detect y error!")
# continue
# # if(trans[2]>-0.04 or trans[2]<-0.08):
# # print("detect z error!")
# # continue
# print(trans)
# # print(rot)
# # 获取当前位姿数据最为机械臂运动的起始位姿
# start_pose = arm.get_current_pose(end_effector_link).pose
# # 初始化路点列表
# waypoints = []
# # 将初始位姿加入路点列表
# waypoints.append(start_pose)
# wpose = deepcopy(start_pose)
# wpose.position.y -= 0.2
# wpose.position.z -= 0.155
# waypoints.append(deepcopy(wpose))
# wpose.position.y -= 0.1
# 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.001, # eef_step，终端步进值
# 0.0, # jump_threshold，跳跃阈值
# True) # avoid_collisions，避障规划
# # 尝试次数累加
# attempts += 1
# # 打印运动规划进程
# if attempts % 10 == 0:
# rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# #print plan
# # 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
# if fraction == 1.0:
# rospy.loginfo("Path computed successfully. Moving the arm.")
# num_of_points = len(plan.joint_trajectory.points)
# d = rospy.Duration.from_sec(3)
# print plan.joint_trajectory.points[num_of_points-1].time_from_start
# for index in range(num_of_points):
# plan.joint_trajectory.points[index].time_from_start *= \
# d/plan.joint_trajectory.points[num_of_points-1].time_from_start
# #print plan
# 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)
# # os.system("python ~/MQTT/ros_gripper_control.py 1100")
# # rospy.sleep(1)
# # 回起始点
# rospy.loginfo("go origin position")
# joint_positions = [-0.007717277486910995, -0.9330994764397906, 0.46196335078534034, 7.155322862129146e-05, -1.08595462478185, -0.007682373472949389]
# arm.set_joint_value_target(joint_positions)
# # 控制机械臂完成运动
# arm.go()
# # # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
# # joint_positions = [-0.8188115183246074, -0.43414310645724263, -0.2707591623036649, -0.008221640488656196, -0.8719249563699826, -0.8136649214659686]
# # arm.set_joint_value_target(joint_positions)
# # # 控制机械臂完成运动
# # arm.go()
# # os.system("python ~/MQTT/ros_gripper_control.py 0")
# # rospy.sleep(1)
# # #回起始点
# # joint_positions = [-3.490401396160559e-05, -0.6098499127399651, 1.0162722513089006, 8.37696335078534e-05, -1.959607329842932, 2.268760907504363e-05]
# # arm.set_joint_value_target(joint_positions)
# # # 控制机械臂完成运动
# # arm.go()
# rospy.sleep(2)
# except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
# continue
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
