This is a demonstration of skills I’ve learned in my robotics class that shows a simulated UR5 robot drawing my initials in the air. It’s controlled by a custom script and math. Based on ROS, Gazebo, and MoveIt.
Motion planning code using MoveIt:
# Python 2/3 compatibility imports from __future__ import print_function from six.moves import input import sys import copy import rospy import moveit_commander import moveit_msgs.msg import geometry_msgs.msg try: from math import pi, tau, dist, fabs, cos except: # For Python 2 compatibility from math import pi, fabs, cos, sqrt tau = 2.0 * pi def dist(p, q): return sqrt(sum((p_i - q_i) ** 2.0 for p_i, q_i in zip(p, q))) from std_msgs.msg import String from moveit_commander.conversions import pose_to_list moveit_commander.roscpp_initialize(sys.argv) rospy.init_node("hw4", anonymous=True) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group_name = "manipulator" move_group = moveit_commander.MoveGroupCommander(group_name) display_trajectory_publisher = rospy.Publisher( "/move_group/display_planned_path", moveit_msgs.msg.DisplayTrajectory, queue_size=20, ) # We can get the name of the reference frame for this robot: planning_frame = move_group.get_planning_frame() print("============ Planning frame: %s" % planning_frame) # We can also print the name of the end-effector link for this group: eef_link = move_group.get_end_effector_link() print("============ End effector link: %s" % eef_link) # We can get a list of all the groups in the robot: group_names = robot.get_group_names() print("============ Available Planning Groups:", robot.get_group_names()) # Sometimes for debugging it is useful to print the entire state of the # robot: joint_goal = move_group.get_current_joint_values() joint_goal[0] = 4.3187 joint_goal[1] = 4.7583 joint_goal[2] = -1.7291 joint_goal[3] = 3.2517 joint_goal[4] = 5.8886 joint_goal[5] = -3.1393 # The go command can be called with joint values, poses, or without any # parameters if you have already set the pose or joint target for the group move_group.go(joint_goal, wait=True) # Calling ``stop()`` ensures that there is no residual movement move_group.stop() pose_goal = geometry_msgs.msg.Pose() pose_goal.position.x = 0.3 pose_goal.position.y = 0.3 pose_goal.position.z = 0.3 move_group.set_pose_target(pose_goal) plan = move_group.go(wait=True) # Calling `stop()` ensures that there is no residual movement move_group.stop() # It is always good to clear your targets after planning with poses. # Note: there is no equivalent function for clear_joint_value_targets() move_group.clear_pose_targets() print("After setting pose") waypoints = [] wpose = move_group.get_current_pose().pose scale = 1; # J ## Move up wpose.position.z += scale * 0.05; #wpose.position.z += scale*0.30; waypoints.append(copy.deepcopy(wpose)); ## Left Top part wpose.position.x -= scale*0.025; waypoints.append(copy.deepcopy(wpose)); ## Right Top part wpose.position.x += scale*0.05; waypoints.append(copy.deepcopy(wpose)); ## Back to middle wpose.position.x -= scale*0.025; waypoints.append(copy.deepcopy(wpose)); ## Back down wpose.position.z -= scale*0.05; waypoints.append(copy.deepcopy(wpose)); ## "hook" part of J wpose.position.x -= scale*0.025; wpose.position.z += scale*0.025; waypoints.append(copy.deepcopy(wpose)); # I ## Move Up wpose.position.z += scale*.05; waypoints.append(copy.deepcopy(wpose)); ## Left Top part wpose.position.x -= scale*0.025; waypoints.append(copy.deepcopy(wpose)); ## Right Top part wpose.position.x += scale*.05; waypoints.append(copy.deepcopy(wpose)); ## Back to middle wpose.position.x -= scale*0.025; waypoints.append(copy.deepcopy(wpose)); ## Back down wpose.position.z -= scale*.05; waypoints.append(copy.deepcopy(wpose)); ## Left Bottom Part wpose.position.x -= scale*0.025; waypoints.append(copy.deepcopy(wpose)); ## Right Bottom Part wpose.position.x += scale*.05; waypoints.append(copy.deepcopy(wpose)); ## Back to middle wpose.position.x -= scale*0.025; waypoints.append(copy.deepcopy(wpose)); # M ## Move up (left part of M) wpose.position.z += scale*.05; waypoints.append(copy.deepcopy(wpose)); ## Down right (moddle part of M) wpose.position.z -= scale*.05; wpose.position.x += scale*.025; waypoints.append(copy.deepcopy(wpose)); ## Back up right (top left) wpose.position.z += scale*.05; wpose.position.x += scale*.025; waypoints.append(copy.deepcopy(wpose)); ## Right side of M (go back down) wpose.position.z -= scale*.05; (plan, fraction) = move_group.compute_cartesian_path( waypoints, 0.01, 0.0 # waypoints to follow # eef_step ) # jump_threshold display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan) # Publish display_trajectory_publisher.publish(display_trajectory) print("before move_group.execute()") move_group.execute(plan, wait=True) print("============ Printing robot state") print(robot.get_current_state()) print("") print("============ Printing robot jacobian") current = move_group.get_current_joint_values() matrix = move_group.get_jacobian_matrix(current) print(matrix) print("")