Robotics Class Demonstration

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("")

 

Leave a Reply

Your email address will not be published.