【FZU-MU】EE413 Assignment

Task 1-3

---

工具安装：
链接：https://pan.baidu.com/s/1mBdDJxKnlG1yogLwtpd1Ig?
提取码：1234
\quad
下面是步骤指导
1：打开catkin_ws文件夹。

2：在该页面点击“”在终端中打开“。

3：在终端里输入

roslaunch ur5_moveit demo_gazebo.launch

回车后界面，打开RVIz：

4：将RVIz中“Planning Group”的属性切换为 ur5_arm

5：在终端中新开一个窗口。

6：在终端新窗口里输入

rosrun ur5_moveit emblem.py

7：成果图

\quad

下面展示Task集成代码emblem.py：

#!/usr/bin/env python3
import sys
import rospy
import moveit_commander
import math
from geometry_msgs.msg import Point
from visualization_msgs.msg import Marker

# Initialization
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('ur5_square_trajectory', anonymous=True)

robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("ur5_arm")

# Marker Initialization and Publisher
marker_publisher = rospy.Publisher('trajectory_marker', Marker, queue_size=100, latch=True)

Markers = []
color = [[0.1, 0.455, 0.54], [1.0, 0.75, 0],[0.635, 0, 0], [0, 0.65, 0.5], [1.0, 1.0, 1.0], [1.0, 1.0, 1.0]]
for i in range(6):
    marker = Marker()
    marker.header.frame_id = "base_link"
    marker.id = i
    marker.type = Marker.LINE_STRIP
    marker.action = Marker.ADD
    marker.pose.orientation.w = 1.0
    if i < 4:
        marker.scale.x = 0.02  # Line width
    else:
        marker.scale.x = 0.03  # Line width
    marker.color.r = color[i][0]
    marker.color.g = color[i][1]
    marker.color.b = color[i][2]
    marker.color.a = 1.0
    marker.points = []
    Markers.append(marker)


# Define the square trajectory in YZ plane
def square_trajectory(side_length, center, marker):
    waypoints = []
    half_side = side_length / 2.0
    corners = [
        (center[0], center[1] - half_side, center[2] + half_side),  # Top Left
        (center[0], center[1] + half_side, center[2] + half_side),  # Top Rightt
        (center[0], center[1] + half_side, center[2] - half_side),  # Bottom Right
        (center[0], center[1] - half_side, center[2] - half_side),  # Bottom Left
        (center[0], center[1] - half_side, center[2] + half_side),  # Back to Top Left to close the square
    ]

    # Create waypoints for the corners of the square
    for corner in corners:
        pose = group.get_current_pose().pose
        pose.position.x = corner[0]
        pose.position.y = corner[1]
        pose.position.z = corner[2]
        waypoints.append(pose)
        # Publish each corner as a marker
        marker.points.append(Point(pose.position.x + 0.2, pose.position.y, pose.position.z - 1.0))

    return waypoints
    
    
# Define the bottom left trapezium trajectory in YZ plane
def left_trapezium_trajectory(side_length, center, marker):
    waypoints = []
    half_side = side_length / 2.0
    corners = [
        (center[0], center[1] - half_side, center[2] + half_side),  # Top Left
        (center[0], center[1] + half_side, center[2] + half_side),  # Top Rightt
        (center[0], center[1] + half_side, center[2] - half_side * 2),  # Bottom Right
        (center[0], center[1] - half_side, center[2] - half_side),  # Bottom Left
        (center[0], center[1] - half_side, center[2] + half_side),  # Back to Top Left to close the square
    ]

    # Create waypoints for the corners of the square
    for corner in corners:
        pose = group.get_current_pose().pose
        pose.position.x = corner[0]
        pose.position.y = corner[1]
        pose.position.z = corner[2]
        waypoints.append(pose)
        # Publish each corner as a marker
        marker.points.append(Point(pose.position.x + 0.2, pose.position.y, pose.position.z - 1.0))

    return waypoints


# Define the bottom right trapezium trajectory in YZ plane
def right_trapezium_trajectory(side_length, center, marker):
    waypoints = []
    half_side = side_length / 2.0
    corners = [
        (center[0], center[1] - half_side, center[2] + half_side),  # Top Left
        (center[0], center[1] + half_side, center[2] + half_side),  # Top Rightt
        (center[0], center[1] + half_side, center[2] - half_side),  # Bottom Right
        (center[0], center[1] - half_side, center[2] - half_side * 2),  # Bottom Left
        (center[0], center[1] - half_side, center[2] + half_side),  # Back to Top Left to close the trapezium
    ]

    # Create waypoints for the corners of the square
    for corner in corners:
        pose = group.get_current_pose().pose
        pose.position.x = corner[0]
        pose.position.y = corner[1]
        pose.position.z = corner[2]
        waypoints.append(pose)
        # Publish each corner as a marker
        marker.points.append(Point(pose.position.x + 0.2, pose.position.y, pose.position.z - 1.0))

    return waypoints 
    
    
# Define the 'M' trajectory in YZ plane
def m_trajectory(height, width, center, marker):
    waypoints = []

    half_width = width / 2.0
    bottom_left = (center[0], center[1] - half_width, center[2] - height)
    top_left = (center[0], center[1] - half_width, center[2])
    middle = (center[0], center[1], center[2] - height / 2)
    top_right = (center[0], center[1] + half_width, center[2])
    bottom_right = (center[0], center[1] + half_width, center[2] - height)

    # Define the points for 'M'
    m_points = [
        bottom_left,  # Start at bottom left
        top_left,     # Draw up to the top left
        middle,       # Draw down to the middle
        top_right,    # Draw up to the top right
        bottom_right  # Draw down to the bottom right
    ]

    # Create waypoints for the 'M'
    for point in m_points:
        pose = group.get_current_pose().pose
        pose.position.x = point[0]
        pose.position.y = point[1]
        pose.position.z = point[2]
        waypoints.append(pose)
        # Publish each point as a marker
        marker.points.append(Point(pose.position.x + 0.2, pose.position.y + 0.4, pose.position.z-1.0))

    return waypoints


# Define the 'U' trajectory in the YZ plane
def u_trajectory(height, width, center, marker):
    waypoints = []

    # Define the bottom and top of the left straight line #1
    top_1 = (center[0], center[1], center[2])
    bottom_1 = (center[0], center[1], center[2] - height + width)

    # Straight line for the leg of the 'U'
    waypoints.append(create_pose(top_1))
    waypoints.append(create_pose(bottom_1))

    # Half-circle for the bottom of the 'U'
    radius = width
    center_of_circle = (bottom_1[0], bottom_1[1] + radius, bottom_1[2])

    for angle in range(180, 0, -10):  # Create the semicircle sideways
        ang = math.radians(angle)
        x = center_of_circle[0]
        y = center_of_circle[1] + radius * math.cos(ang)
        z = center_of_circle[2] - radius * math.sin(ang)  # Minus because we go down from the center
        waypoints.append(create_pose((x, y, z)))


    # Define the bottom and top of the right straight line #2
    bottom_2 = (center[0], center[1] + radius * 2, center[2] - height + width)
    top_2 = (center[0], center[1] + radius * 2, center[2])
    
    # Straight line for the leg of the 'U'
    waypoints.append(create_pose(bottom_2))
    waypoints.append(create_pose(top_2))

    # Publish the marker points
    for waypoint in waypoints:
        marker.points.append(Point(waypoint.position.x + 0.2, waypoint.position.y + 0.6, waypoint.position.z-1.0))

    return waypoints


# Helper function to create a pose from a point
def create_pose(point):
    pose = group.get_current_pose().pose
    pose.position.x = point[0]
    pose.position.y = point[1]
    pose.position.z = point[2]
    return pose


# Command the UR5 to follow the trajectory
def execute_trajectory(waypoints):
    (plan, fraction) = group.compute_cartesian_path(
                            waypoints,
                            0.01,  # eef_step
                            0.0)   # jump_threshold

    if fraction == 1.0:
        print("Full trajectory computed!")
        group.execute(plan, wait=True)
    else:
        print("Could not compute full trajectory. Only " + str(fraction * 100) + "% achieved.")


# Main Execution
if __name__ == '__main__':
    current_pose = group.get_current_pose().pose.position
    side_length = 0.1  # Change this to the desired height
    height = 0.2
    m_width = 0.2   # Change this to the desired width
    u_width = 0.1  # Change this to the desired width

    point_1 = [current_pose.x, current_pose.y, current_pose.z]
    point_2 = [current_pose.x, current_pose.y + 0.11, current_pose.z]
    point_3 = [current_pose.x, current_pose.y + 0.11, current_pose.z - 0.09]
    point_4 = [current_pose.x, current_pose.y, current_pose.z-0.09]
    point = [current_pose.x, current_pose.y, current_pose.z + 0.06] 
    
    drawn = False


    while not rospy.is_shutdown():
        if not drawn:
            # Drawing the two top squares
            waypoints = square_trajectory(side_length, point_1, Markers[0])
            execute_trajectory(waypoints)
            
            waypoints = square_trajectory(side_length, point_2, Markers[1])
            execute_trajectory(waypoints)
            
            # Drawing the two bottom trapeziums
            waypoints = right_trapezium_trajectory(side_length, point_3, Markers[2])
            execute_trajectory(waypoints)
            
            waypoints = left_trapezium_trajectory(side_length, point_4, Markers[3])
            execute_trajectory(waypoints)
            
            # Drawing the 'M', 'U'
            waypoints = m_trajectory(height, m_width, point, Markers[4])
            execute_trajectory(waypoints)
            
            waypoints = u_trajectory(height, u_width, point, Markers[5])
            execute_trajectory(waypoints)
            
            
            # Drawing the logo trajectory
            marker_publisher.publish(Markers[0])
            marker_publisher.publish(Markers[1])
            marker_publisher.publish(Markers[2])
            marker_publisher.publish(Markers[3])
            
            # Drawing the M trajectory
            marker_publisher.publish(Markers[4])
            
            # Drawing the U trajectory
            marker_publisher.publish(Markers[5])

            drawn = True
            break
            
        rospy.sleep(5)