def genCommand(char, command):
"""Update the command according to the character entered by the user."""
if char == 'a':
command = outputMsg.CModel_robot_output();
command.rACT = 1
command.rGTO = 1
command.rSP = 255
command.rFR = 150
if char == 'r':
command = outputMsg.CModel_robot_output();
command.rACT = 0
if char == 'c':
command.rPR = 255
if char == 'o':
command.rPR = 0
#If the command entered is a int, assign this value to rPRA
try:
command.rPR = int(char)
if command.rPR > 255:
command.rPR = 255
if command.rPR < 0:
command.rPR = 0
except ValueError:
pass
if char == 'f':
command.rSP += 25
if command.rSP > 255:
command.rSP = 255
if char == 'l':
command.rSP -= 25
if command.rSP < 0:
command.rSP = 0
if char == 'i':
command.rFR += 25
if command.rFR > 255:
command.rFR = 255
if char == 'd':
command.rFR -= 25
if command.rFR < 0:
command.rFR = 0
return command
def activateGripper(self):
self.command = outputMsg.CModel_robot_output()
self.command.rACT = 1
self.command.rGTO = 1
self.command.rSP = 255
self.command.rFR = 150
self.publish()
def __init__(self, gripper_name):
# input gACT gGTO gSTA gOBJ gFLT gPR gPO gCU
self.query = inputMsg.CModel_robot_input()
# self.activated = False
self.current = 0.0
self.requested_pos = 0
self.curr_pos = 0
# gOBJ: Only valid if gGTO = 1.
# 0x00 - Fingers are in motion towards requested position. No object detected.
# 0x01 - Fingers have stopped due to a contact while opening before requested position. Object detected.
# 0x02 - Fingers have stopped due to a contact while closing before requested position. Object detected.
# 0x03 - Fingers are at requested position. No object detected or object has been lost / dropped.
self.grasp_status = 0
# output_msg rACT rGTO rATR rPR rSP rFR
self.command = outputMsg.CModel_robot_output()
self.command_pub = rospy.Publisher(
gripper_name + '/output', outputMsg.CModel_robot_output, queue_size=1)
print("init gripper")
self.reset()
sleep(1)
self.activate()
sleep(1)
print("finished")
self.query_sub = rospy.Subscriber(
gripper_name + '/input', inputMsg.CModel_robot_input, self.query_callback)
def set_gripper_position(position): ##0=open, 255=close
command = outputMsg.CModel_robot_output()
command.rPR = position
gripper_publisher.publish(command)
print "Gripper Position:"
print position
return command
def getDefaultMsg():
command = outputMsg.CModel_robot_output()
command.rACT = 1
command.rGTO = 1
command.rSP = 255
command.rFR = 150
return command
def generateActivateCommand():
command = outputMsg.CModel_robot_output()
command.rACT = 1
command.rGTO = 1
command.rSP = 255
command.rFR = 150
return command
def __init__(self):
self.command = outputMsg.CModel_robot_output()
self.pub = rospy.Publisher('CModelRobotOutput',
outputMsg.CModel_robot_output,
queue_size=10)
self.command.rACT = 1
self.command.rGTO = 1
self.command.rSP = 205
self.command.rFR = 150
def gactive(pub):
command = outputMsg.CModel_robot_output()
command.rACT = 1
command.rGTO = 1
command.rSP = 50
command.rFR = 150
pub.publish(command)
rospy.sleep(0.5)
return command
def activate_gripper():
command = outputMsg.CModel_robot_output()
command.rACT = 1
command.rGTO = 1
command.rSP = 50
command.rFR = 150 ##force need to be adjusted later
gripper_publisher.publish(command)
rospy.sleep(2)
print "Gripper Activated"
return command
def gposition(pub, command, position): ##0=open, 255=close
#rospy.sleep(0.5)
command = outputMsg.CModel_robot_output()
command.rACT = 1
command.rGTO = 1
command.rSP = 50
command.rFR = 150
command.rPR = position
pub.publish(command)
return command
def activate(self):
# if not self.activated:
print('activate')
self.command = outputMsg.CModel_robot_output()
self.command.rACT = 1
self.command.rGTO = 1
self.command.rSP = 255
self.command.rFR = 150
self.command_pub.publish(self.command)
self.activated = True
def gen_command(self, char):
"""Update the command according to the character entered by the user."""
if char == 'a':
self.command = outputMsg.CModel_robot_output()
self.command.rACT = 1
self.command.rGTO = 1
self.command.rSP = 255
self.command.rFR = 150
if char == 'r':
self.command = outputMsg.CModel_robot_output()
self.command.rACT = 0
if char == 'c':
self.command.rPR = 255
if char == 'o':
self.command.rPR = 0
# If the self.command entered is a int, assign this value to rPRA
try:
self.command.rPR = int(char)
self.command.rPR = min(self.command.rPR, 255)
self.command.rPR = max(self.command.rPR, 0)
except ValueError:
pass
if char == 'f':
self.command.rSP += 25
self.command.rSP = min(self.command.rSP, 255)
if char == 'l':
self.command.rSP -= 25
self.command.rSP = max(self.command.rSP, 0)
if char == 'i':
self.command.rFR += 25
self.command.rFR = min(self.command.rFR, 255)
if char == 'd':
self.command.rFR -= 25
self.command.rFR = max(self.command.rFR, 0)
def run():
pub = rospy.Publisher('CModelRobotOutput', outputMsg.CModel_robot_output)
command = outputMsg.CModel_robot_output()
print "============ setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('CoffeeMakingByXYZ', anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("manipulator")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
## print Basic Information
print "============ Reference frame: %s" % group.get_planning_frame()
print "============ End effector: %s" % group.get_end_effector_link()
print "============ Robot Groups:"
print robot.get_group_names()
print "Printing robot state:"
print robot.get_current_state()
print "ready?"
raw_input() ##Safety@Sampson
group.set_max_acceleration_scaling_factor(0.5)
main_program(group, pub, command)
##better use a while loop to call test when user input
print "finished"
## Adding/Removing Objects and Attaching/Detaching Objects
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## First, we will define the collision object message
collision_object = moveit_msgs.msg.CollisionObject()
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
print "============ STOPPING"
def publisher():
"""Main loop which requests new commands and publish them on the CModelRobotOutput topic."""
rospy.init_node('CModelSimpleController')
pub = rospy.Publisher('CModelRobotOutput', outputMsg.CModel_robot_output)
command = outputMsg.CModel_robot_output();
while not rospy.is_shutdown():
command = genCommand(askForCommand(command), command)
pub.publish(command)
rospy.sleep(0.1)
def reset(self, msg=None):
self.command = outputMsg.CModel_robot_output()
self.command.rACT = 0
self.pub.publish(self.command)
return []
def greset(pub):
command = outputMsg.CModel_robot_output()
command.rACT = 0
pub.publish(command)
rospy.sleep(0.5)
def reset_gripper():
command = outputMsg.CModel_robot_output()
command.rACT = 0
gripper_publisher.publish(command)
print "Gripper Reset"
def generateResetCommand():
command = outputMsg.CModel_robot_output()
command.rACT = 0
return command
def reset(self):
print('reset')
self.command = outputMsg.CModel_robot_output()
self.command.rACT = 0
self.command_pub.publish(self.command)
def resetGripper(self):
self.command = outputMsg.CModel_robot_output()
self.command.rACT = 0
self.publish()
def pickup(command, distance, vel):
rospy.sleep(0.5)
gposition(pub, command, 150) #increment gripper width
rospy.sleep(1)
resolution = 0.05 #resolution is interpreted as 1/resolution = number of interpolated points in the path
pose_target = group.get_current_pose().pose
x_1 = pose_target.position.x
y_1 = pose_target.position.y
z_1 = pose_target.position.z
x_2 = x_1
y_2 = y_1
z_2 = z_1 + distance
direction_vector = [x_2-x_1, y_2-y_1, z_2-z_1]
pose_target = group.get_current_pose().pose #create a pose variable. The parameters can be seen from "$ rosmsg show Pose"
waypoints = []
waypoints.append(pose_target)
t = 0 # counter/increasing variabe for the parametric equation of straight line
while t <= 1.01:
pose_target.position.x = x_1 + direction_vector[0]*t
pose_target.position.y = y_1 + direction_vector[1]*t
pose_target.position.z = z_1 + direction_vector[2]*t
t += resolution
waypoints.append(copy.deepcopy(pose_target))
del waypoints[:1]
plan_execute_waypoints(waypoints)
command = outputMsg.CModel_robot_output();
command.rACT = 1
command.rGTO = 1
command.rSP = 20
command.rFR = 150 ##force need to be adjusted later
command.rPR = 220
pub.publish(command)
rospy.sleep(1)
pose_target = group.get_current_pose().pose
x_1 = pose_target.position.x
y_1 = pose_target.position.y
z_1 = pose_target.position.z
x_2 = x_1
y_2 = y_1
z_2 = z_1 + 0.1
direction_vector = [x_2-x_1, y_2-y_1, z_2-z_1]
pose_target = group.get_current_pose().pose #create a pose variable. The parameters can be seen from "$ rosmsg show Pose"
waypoints = []
waypoints.append(pose_target)
t = 0 # counter/increasing variabe for the parametric equation of straight line
while t <= 1.01:
pose_target.position.x = x_1 + direction_vector[0]*t
pose_target.position.y = y_1 + direction_vector[1]*t
pose_target.position.z = z_1 + direction_vector[2]*t
t += resolution
waypoints.append(copy.deepcopy(pose_target))
del waypoints[:1]
plan_execute_waypoints(waypoints)
#!/usr/bin/env python
import roslib
roslib.load_manifest('robotiq_c_model_control')
from robotiq_c_model_control.msg import _CModel_robot_output as outputMsg
import rospy
import time
from std_msgs.msg import UInt8
from geometry_msgs.msg import Vector3
command = outputMsg.CModel_robot_output()
pub = rospy.Publisher('CModelRobotOutput',
outputMsg.CModel_robot_output,
queue_size=10)
def gen_command(gest, command):
"""Generates a gripper command given a certain gesture"""
if gest == 6: #activate
command.rACT = 1
command.rGTO = 1
command.rSP = 105
command.rFR = 25
if gest == 7: #reset
command.rACT = 0
if gest == 4: #open open palm
command.rPR = 0
if gest == 20: #close
command.rPR = 255
def __init__(self):
rospy.init_node("grasp_object", anonymous=False)
rospy.loginfo("Starting node grasp_object")
rospy.on_shutdown(self.cleanup)
listener = tf.TransformListener()
gripperpub = rospy.Publisher('CModelRobotOutput',
outputMsg.CModel_robot_output)
# Activate gripper
# rACT: 1
msg = outputMsg.CModel_robot_output()
#msg.rACT = 1
msg.rACT = 1
msg.rGTO = 1
msg.rATR = 0
msg.rPR = 0
msg.rSP = 255
msg.rFR = 150
gripperpub.publish(msg)
rospy.sleep(0.1)
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the move group for the ur5_arm
self.arm = moveit_commander.MoveGroupCommander('manipulator')
# Get the name of the end-effector link
end_effector_link = self.arm.get_end_effector_link()
# Set the reference frame for pose targets
reference_frame = "/base_link"
# Set the ur5_arm reference frame accordingly
self.arm.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
self.arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
self.arm.set_goal_position_tolerance(0.005)
self.arm.set_goal_orientation_tolerance(0.05)
self.arm.set_max_velocity_scaling_factor(0.1)
while True:
x = raw_input('waitkey(o open gripper): ')
if x == 'o':
msg.rACT = 1
msg.rGTO = 1
msg.rATR = 0
msg.rPR = 0
msg.rSP = 255
msg.rFR = 150
gripperpub.publish(msg)
rospy.sleep(0.1)
break
# Get the current pose so we can add it as a waypoint
start_pose = self.arm.get_current_pose(end_effector_link).pose
#print "start pose", start_pose
#listener.waitForTransform("/base_link", "/tag_0", rospy.Time(), rospy.Duration(4.0))
#listener.waitForTransform("/base_link", "/tool0_controller", rospy.Time(), rospy.Duration(4.0))
listener.waitForTransform("/base_link", "/dove_tag_middle",
rospy.Time(), rospy.Duration(4.0))
running = True
while running:
try:
now = rospy.Time.now()
#listener.waitForTransform("/base_link", "/tag_0", now, rospy.Duration(4.0))
listener.waitForTransform("/base_link", "/dove_tag_middle",
now, rospy.Duration(4.0))
#listener.waitForTransform("/base_link", "/tool0_controller", now, rospy.Duration(4.0))
#(translation, quaternion) = listener.lookupTransform("/base_link", "/tag_0", now)
(translation, quaternion) = listener.lookupTransform(
"/base_link", "/dove_tag_middle_mod", now)
#(translationdd, quaterniondd) = listener.lookupTransform("/base_link", "/dove_tag_middle_mod2", now)
#(translation_inv, quaternion_inv) = listener.lookupTransform("/dove_tag_middle", "/base_link", now)
#(translation, quaternion) = listener.lookupTransform("/base_link", "/tool0_controller", now)
#transform = tfm.concatenate_matrices(tfm.translation_matrix(translation), tfm.quaternion_matrix(quaternion))
#inversed_transform = tfm.inverse_matrix(transform)
#translationqq = tfm.translation_from_matrix(inversed_transform)
#quaternionqq = tfm.quaternion_from_matrix(inversed_transform)
#print quaternionqq
#(translation_d1, quaternion_d1) = listener.lookupTransform("/base_link", "/gripper", now)
#(translation_d2, quaternion_d2) = listener.lookupTransform("/base_link", "/tool0_controller", now)
#(translation_dd, quaternion_dd) = listener.lookupTransform("/gripper", "/tool0_controller", now)
#(translation_middle, quaternion_middle) = listener.lookupTransform("/base_link", "/dove_tag_middle", now)
running = False
except (tf.Exception), e:
print "transform not available, aborting..."
print "Error message: ", e
