def __init__(self):
self.c = 0
self.command = outputMsg.SModel_robot_output()
self.pub = rospy.Publisher('SModelRobotOutput',
outputMsg.SModel_robot_output)
self.subonce = None
self.rarm = moveit_commander.MoveGroupCommander("manipulator")
self.robot = moveit_commander.RobotCommander()
self.xlimit = [-0.60, -0.30]
self.ylimit = [0.360, 0.60]
self.z = 0.50
self.z_min = 0.135
self.sleeptime = 0.5
self.gripper_close_ratio = 77
self.image_topic = "/camera/rgb/image_color"
gripper_pose_virtical = tf.transformations.quaternion_from_euler(
-pi / 2, pi / 2, pi / 2)
self.home_pose = geometry_msgs.msg.Pose()
self.home_pose.position.x = -0.304788039946
self.home_pose.position.y = 0.401533995393
self.home_pose.position.z = 0.497607185593
self.home_pose.orientation.x = gripper_pose_virtical[0]
self.home_pose.orientation.y = gripper_pose_virtical[1]
self.home_pose.orientation.z = gripper_pose_virtical[2]
self.home_pose.orientation.w = gripper_pose_virtical[3]
self.workspace = [-1.0, 0.0, 0.1, 0.1, 1.0, 1.5]
def getDefaultMsg():
command = outputMsg.SModel_robot_output()
command.rACT = 1
command.rGTO = 1
command.rSPA = 255
command.rFRA = 150
return command
def publisher(keep_gripper_status):
global command_msg, newCommandAvailable, force_pub
"""Main loop which subscribes to new commands and publish them on the SModelRobotOutput topic."""
rospy.init_node('SModelPublisherController')
command_pub = rospy.Publisher('SModelRobotOutput',
outputMsg.SModel_robot_output,
queue_size=10)
command_force_sub = rospy.Subscriber('SModelRobotInput',
inputMsg.SModel_robot_input, force_cb)
command_sub = rospy.Subscriber('GripperCommand', String, cb)
force_pub = rospy.Publisher('/GripperForceFeedback', String, queue_size=10)
command = outputMsg.SModel_robot_output()
firstTime = True
newMessageAvailable = False
sent_command = False
while not rospy.is_shutdown():
if firstTime:
# Activate the gripper.
if keep_gripper_status == 'n':
rospy.loginfo("Reactivating the gripper...")
command = genCommand('a', command)
newMessageAvailable = True
firstTime = False
else:
if newCommandAvailable:
try:
command_int = int(command_msg)
if (command_int < 120):
exit_cause = True
while (exit_cause):
if command_pub.get_num_connections() > 0:
command_pub.publish('i')
newMessageAvailable = False
rospy.sleep(0.1)
exit_cause = (not exceeded_force)
sent_command = True
except:
pass
if not sent_command:
command = genCommand(command_msg, command)
newCommandAvailable = False
newMessageAvailable = True
if not sent_command and newMessageAvailable and command_pub.get_num_connections(
) > 0:
command_pub.publish(command)
newMessageAvailable = False
rospy.sleep(0.1)
rospy.spin()
def __init__(self):
self.grip_command = outputMsg.SModel_robot_output()
self.pub_gripper = rospy.Publisher('SModelRobotOutput',
outputMsg.SModel_robot_output,
queue_size=1)
self.sub_gripper = rospy.Subscriber('SModelRobotInput',
inputMsg.SModel_robot_input,
self.get_gripper_msg)
self.gripper_goal = "o"
self.gripper_msg_POA = 0
def publisher():
"""Main loop which requests new commands and publish them on the SModelRobotOutput topic."""
rospy.init_node('gripper_service')
command = outputMsg.SModel_robot_output()
s = rospy.Service('command_gripper', gripper, genCommand)
rospy.spin()
def genCommand(self, char, command):
"""Update the command according to the character entered by the user."""
if char == 'a':
command = outputMsg.SModel_robot_output()
command.rACT = 1
command.rGTO = 1
command.rSPA = 255
command.rFRA = 150
if char == 'r':
command = outputMsg.SModel_robot_output()
command.rACT = 0
if char == 'c':
command.rPRA = 255
if char == 'o':
command.rPRA = 0
return command
def __init__(self):
self.gripper_status = 0
self.sub_gripper = rospy.Subscriber("SModelRobotInput",
inputMsg.SModel_robot_input,
self.gripper_status_cb)
self.grip_command = outputMsg.SModel_robot_output()
self.pub_gripper = rospy.Publisher("SModelRobotOutput",
outputMsg.SModel_robot_output,
queue_size=1)
self.gripper_goal = "o"
self.rate = rospy.Rate(10)
self.service = rospy.Service('gripper_command', ControlGripper,
self.handle_gripper_command)
print "initialized"
def do_action(self, target):
# global q_star
# global q_send
# global q_speed
global positionReached
global currentAction
# global actionPhase
global count
rtol = 1e-2
atol = 1e-2
r = rospy.Rate(controller.ctrl_freq)
joint_states_msg = rospy.wait_for_message('iiwa/joint_states',
numpy_msg(JointState))
current_position = joint_states_msg.position
# q_star = current_position
self.q_send = current_position
# q_speed = current_position*0.005
self.lin_ds(current_position, target)
self.send_cmd(self.q_send)
if np.allclose(target[2:7],current_position[2:7], rtol, atol) and ((currentTarget==jointPositionsP1).all() or \
(currentTarget==jointPositionsP2).all() or (currentTarget==jointPositionsP3).all()) :
positionReached = 1
grip_command = outputMsg.SModel_robot_output()
# close gripper
grip_command.rACT = 1
grip_command.rGTO = 1
grip_command.rSPA = 255
grip_command.rFRA = 150
grip_command.rPRA = 255
print(len(graphs.commandHistoricJoint0))
print(graphs.commandHistoricJoint0f)
# self.pubGripper.publish(command)
# gripper_msg = rospy.wait_for_message('SModelRobotInput', inputMsg.SModel_robot_input)
# while gripper_msg.gPOA < 200 :
# gripper_msg = rospy.wait_for_message('SModelRobotInput', inputMsg.SModel_robot_input)
# print(gripper_msg.gPOA)
elif positionReached == 1 and np.allclose(
jointPositionsHome[2:7], current_position[2:7], rtol, atol):
positionReached = 0
count = 1
graphs.plot_cmd_error(graphs.commandHistoricJoint0,
graphs.errorJoint0)
r.sleep()
def publisher():
rospy.init_node('CloseGripper')
pub = rospy.Publisher('SModelRobotOutput',
outputMsg.SModel_robot_output,
queue_size=1)
command = outputMsg.SModel_robot_output()
command.rACT = 1
command.rGTO = 1
command.rSPA = 255
command.rFRA = 150
command.rPRA = 0
rospy.sleep(1)
pub.publish(command)
def publisher():
"""Main loop which requests new commands and publish them on the SModelRobotOutput topic."""
rospy.init_node('SModelSimpleController')
pub = rospy.Publisher('SModelRobotOutput', outputMsg.SModel_robot_output)
command = outputMsg.SModel_robot_output()
while not rospy.is_shutdown():
command = genCommand(askForCommand(command), command)
pub.publish(command)
rospy.sleep(0.1)
def __init__(self, model, lexicon, kwlist, pub_string):
# initialize ROS
self.msg_string = String()
self.msg_gripper = outputMsg.SModel_robot_output()
self.gripper_state = 0
rospy.on_shutdown(self.shutdown)
# you may need to change publisher destination depending on what you run
self.pub_string = rospy.Publisher(pub_string, String, queue_size=10)
self.pub_gripper = rospy.Publisher('SModelRobotOutput',
outputMsg.SModel_robot_output)
rospy.sleep(2)
# Activate gripper
self.msg_gripper = self.genCommand("r", self.msg_gripper)
self.pub_gripper.publish(self.msg_gripper)
rospy.sleep(1)
print "Activating Gripper\n"
self.msg_gripper = self.genCommand("a", self.msg_gripper)
self.pub_gripper.publish(self.msg_gripper)
rospy.sleep(1)
# initialize pocketsphinx
config = Decoder.default_config()
config.set_string('-hmm', model)
config.set_string('-dict', lexicon)
config.set_string('-kws', kwlist)
stream = pyaudio.PyAudio().open(format=pyaudio.paInt16,
channels=1,
rate=16000,
input=True,
frames_per_buffer=1024)
stream.start_stream()
self.decoder = Decoder(config)
self.decoder.start_utt()
while not rospy.is_shutdown():
buf = stream.read(1024)
if buf:
self.decoder.process_raw(buf, False, False)
else:
break
self.parse_asr_result()
def __init__(self):
print('Start node')
# Introduce variables to make sure at least one message is recieved
#self.awaitJointPos=True # Get first attractor obstacle
self.awaitEE = True
self.awaitingTrafo_ee = True
# Pickup positions
#self.pos_pickup = [[-0.060, 0.70, 0.34],
#[-0.618,-0.267,0.214]]
self.pos_pickup = [[-0.44, 0.61, 0.34]]
self.pos_dropoff = [[-.272, -0.420, 0.30], [-0.618, -0.267, 0.214]]
self.quat_dropoff = [0, 1, 0, 0]
# Resting position & position between drop-off and pick up
self.restEul = [0, 0, 0]
self.restPos = [-0.5, 0, 0.6]
# z direction world Frame
self.zDir_des = [0, 0, -1]
#self.pickQuat = [0,1,0,0] # Pickup orientation
# Normalize
zDir_des_norm = np.linalg.norm(self.zDir_des)
if zDir_des_norm: # normalize if non zerovalue
self.zDir_des = np.array(self.zDir_des) / zDir_des_norm
# Resting quaternion
#self.restQuat = [1,0,0,0]
#print(tf.transformations.quaternion_matrix(self.restQuat))
#self.joinState_init = [0,0,0,120*pi/180,0,60*pi/180,0]
self.n_joints = 7
# Initialize node
rospy.init_node('attractor_publisher', anonymous=True)
rate = rospy.Rate(10) # Frequency
rospy.on_shutdown(self.end_mode)
self.pub_attr = rospy.Publisher('attractor', PoseStamped, queue_size=5)
self.pub_grip = rospy.Publisher('SModelRobotOutput',
outputMsg.SModel_robot_output,
queue_size=10) # Gripper publisher
# self.pub_joint = rospy.Publisher('lwr/joint_controllers/command_joint_pos', Float64MultiArray, queue_size=10)
# subscriber
self.sub_joint = rospy.Subscriber("lwr/ee_pose", Pose,
self.callback_joint)
while self.awaitEE:
rospy.loginfo('Waiting for EE-Pose')
rospy.sleep(0.1)
self.listener = tf.TransformListener() # TF listener
while (self.awaitingTrafo_ee): # TRAFO robot position
try: # Get transformation
self.pos_rob, self.pos_rob = self.listener.lookupTransform(
"/world", "/lwr_7_link", rospy.Time(0))
self.awaitingTrafo_ee = False
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.loginfo('Waiting for Robot-EE Tranformation.')
rospy.sleep(0.2) # Wait zzzz*
self.grippingDuration = 1 # Gripping during 0.5 s
self.t_grippingStart = -1
self.dist_startGripping = 0.08 # Distance at which gripping starts
self.deltaJoint_err = 0.003
self.controlMode = 'default' # initial control mode
#self.controlMode = 'dropoff' # initial control mode
self.activateGripper = False
self.dist_rangeMax = 1.
self.it_pick = -1
self.gripCommand = outputMsg.SModel_robot_output()
# Gripper command
# set gripper init command -- Initalize gripper
self.command = outputMsg.SModel_robot_output()
self.command.rACT = 1 # ?? -- rAct=0 (reset) // rAct=1 (activate)
self.command.rGTO = 1 # ??
self.command.rSPA = 255 # gripper speed (0-255)
self.command.rFRA = 50 # gripper forcee (0-255)
self.command.rMOD = 1 # resolution of control
self.command.rPRA = 255 # gripper position - open
rospy.loginfo("Entering High Level Control Loop.")
while not rospy.is_shutdown():
try: # Get transformation
self.pos_rob, self.quat_rob = self.listener.lookupTransform(
"/world", "/lwr_7_link", rospy.Time(0))
except:
rospy.loginfo("No <</lwr_7_link>> recieved")
##################### DEFAULT MODE ####################
if self.controlMode == 'default':
self.mode_default()
distGoal = np.sqrt(
np.sum(
(np.array(self.pos_rob) - np.array(self.restPos))**2))
#deltaJoint = 0
if distGoal < self.dist_startGripping:
if len(self.pos_pickup) > 1 + self.it_pick:
#self.it_pick = self.it_pick + 1 # INFINITE LOOP WITH SAME POSITION IF ITERATION OFF
self.it_pick = max(self.it_pick, 0)
print('Going for obstacle {}'.format(1 + self.it_pick))
self.controlMode = 'pickup_xy'
print('Preparing pickup')
self.send_gripperCommand(
'open') # check and send gripper command
# self.pub_joint.publish(Float64MultiArray()) # zero joint command
else:
print('Mission complete - shutting down.')
rospy.signal_shutdown('Mission complete.')
##################### PICKUP MODE -- set only x and y ####################
if self.controlMode == 'pickup_xy': #higher up
self.mode_default()
dZ = 0.15
pos_startPick = [
self.pos_pickup[self.it_pick][0],
self.pos_pickup[self.it_pick][1],
self.pos_pickup[self.it_pick][2] + dZ
]
self.mode_pickup(dZ)
distGoal = np.sqrt(
np.sum(
(np.array(self.pos_rob) - np.array(pos_startPick))**2))
#deltaJoint = 0
if distGoal < self.dist_startGripping:
self.controlMode = 'pickup'
print('Starting pickup.')
# self.pub_joint.publish(Float64MultiArray()) # zero joint command
#################### PICKUP MODE -- set x, y AND z ####################
elif self.controlMode == 'pickup':
#print('now pickup mode')
self.mode_pickup()
if not self.activateGripper:
#self.it_pick = 0 # could be changed for different positions
distGoal = np.sqrt(
np.sum((self.pos_rob -
np.array(self.pos_pickup[self.it_pick]))**2))
self.send_gripperCommand(
'open') # check and send gripper command\
if distGoal < self.dist_startGripping:
self.activateGripper = True
self.t_grippingStart = rospy.Time.now().to_sec()
else: # Gripper activated
if self.activateGripper:
dt_gripping = rospy.Time.now().to_sec(
) - self.t_grippingStart
self.send_gripperCommand(
'close') # check and send gripper command
if dt_gripping > self.grippingDuration:
#self.controlMode = 'dropoff_xy'
self.controlMode = 'dropoff'
print('Preparing dropoff.')
self.activateGripper = False
dt_gripping = 0
##################### MODE PRE DROPOFF -- set only x and y ####################
if self.controlMode == 'dropoff_xy': #higher up
dZ = 0.2 # DELTA Z
pos_goal = np.array(self.pos_dropoff[self.it_pick]) + np.array(
[0, 0, dZ])
self.mode_dropoff(pos_goal)
distGoal = np.sqrt(
np.sum((self.pos_rob - np.array(pos_goal))**2))
if distGoal < self.dist_startGripping:
self.controlMode = 'dropoff'
print('Dropping off now..')
#################### DROP OFF ####################
elif self.controlMode == 'dropoff':
self.attrObs_quat = self.quat_dropoff
# Check if attractor in in range
# if np.sqrt(np.sum(np.array(self.pos_attr)**2)) > self.dist_rangeMax:
# print('WARNING -- Drop-off location too far way. I go home.')
# self.mode_dropoff(self.restPos) # TODO uncomment for test
#self.mode_dropoff(pos_goal)
# else:
# self.mode_dropoff(pos_goal)
pos_goal = np.array(self.pos_dropoff[self.it_pick])
self.mode_dropoff(pos_goal)
if not self.activateGripper:
distGoal = np.sqrt(
np.sum((self.pos_rob - np.array(pos_goal))**2))
self.send_gripperCommand(
'close') # check and send gripper command
if distGoal < self.dist_startGripping:
self.activateGripper = True
self.t_grippingStart = rospy.Time.now().to_sec()
else: # Gripper activated
if self.activateGripper:
dt_gripping = rospy.Time.now().to_sec(
) - self.t_grippingStart
self.send_gripperCommand(
'open') # check and send gripper command
if dt_gripping > self.grippingDuration:
#self.controlMode = 'dropoff_post' # TODO uncomment for test
#self.controlMode = 'default' # TODO uncomment for test
self.controlMode = 'pickup_xy' # TODO uncomment for test
self.activateGripper = False
print('Entering default mode!')
dt_gripping = 0
##################### MODE POST-DROPOFF -- set only x and y ####################
if self.controlMode == 'dropoff_post': #higher up
self.attrObs_quat = self.quat_dropoff
dZ = 0.2 # DELTA Z
pos_goal = np.array(self.pos_dropoff[self.it_pick]) + np.array(
[0, 0, dZ])
self.mode_dropoff(pos_goal)
distGoal = np.sqrt(
np.sum((self.pos_rob - np.array(pos_goal))**2))
if distGoal < self.dist_startGripping:
self.controlMode = 'default'
print('Dropping off finished..')
rate.sleep()
def genCommand(char, command):
"""Update the command according to the character entered by the user."""
if char == 'a':
command = outputMsg.SModel_robot_output()
command.rACT = 1
command.rGTO = 1
command.rSPA = 255
command.rFRA = 150
if char == 'r':
command = outputMsg.SModel_robot_output()
command.rACT = 0
if char == 'c':
command.rPRA = 255
if char == 'o':
command.rPRA = 0
if char == 'b':
command.rMOD = 0
if char == 'p':
command.rMOD = 1
if char == 'w':
command.rMOD = 2
if char == 's':
command.rMOD = 3
if char == 'K':
command.rPRA = 80
# If the command entered is a int, assign this value to rPRA
try:
command.rPRA = int(char)
if command.rPRA > 255:
command.rPRA = 255
if command.rPRA < 0:
command.rPRA = 0
except ValueError:
pass
if char == 'f':
command.rSPA += 25
if command.rSPA > 255:
command.rSPA = 255
if char == 'l':
command.rSPA -= 25
if command.rSPA < 0:
command.rSPA = 0
if char == 'i':
command.rFRA += 25
if command.rFRA > 255:
command.rFRA = 255
if char == 'd':
command.rFRA -= 25
if command.rFRA < 0:
command.rFRA = 0
return command
rospy.init_node('SModelSimpleController', anonymous=True)
pub = rospy.Publisher('left/SModelRobotOutput',
outputMsg.SModel_robot_output,
queue_size=10)
pub2 = rospy.Publisher('right/SModelRobotOutput',
outputMsg.SModel_robot_output,
queue_size=10)
rospy.Subscriber('left_gripper_signal', String, callback_left)
rospy.Subscriber('right_gripper_signal', String, callback_right)
rospy.loginfo("Begin!")
command = outputMsg.SModel_robot_output()
while not rospy.is_shutdown():
command = genCommand(str, command)
if enable_arm == True:
pub.publish(command)
else:
pub2.publish(command)
rospy.sleep(0.1)
rospy.spin()
def reset(self, msg=None):
self.command = outputMsg.SModel_robot_output()
self.command = outputMsg.SModel_robot_output()
self.command.rACT = 0
self.pub.publish(self.command)
return []
def genCommand(action, command):
"""Update the command according to the character entered by the user."""
if action == 'activate':
command = outputMsg.SModel_robot_output()
command.rACT = 1
command.rGTO = 1
command.rSPA = 255
command.rFRA = 150
if action == 'reset':
command = outputMsg.SModel_robot_output()
command.rACT = 0
if action == 'close':
command.rPRA = 255
if action == 'open':
command.rPRA = 0
if action == 'basic':
command.rMOD = 0
if action == 'pinch':
command.rMOD = 1
if action == 'wide':
command.rMOD = 2
if action == 'scisor':
command.rMOD = 3
#If the command entered is a int, assign this value to rPRA
try:
command.rPRA = int(action)
if command.rPRA > 255:
command.rPRA = 255
if command.rPRA < 0:
command.rPRA = 0
except ValueError:
pass
if action == 'f':
command.rSPA += 25
if command.rSPA > 255:
command.rSPA = 255
if action == 'l':
command.rSPA -= 25
if command.rSPA < 0:
command.rSPA = 0
if action == 'i':
command.rFRA += 25
if command.rFRA > 255:
command.rFRA = 255
if action == 'd':
command.rFRA -= 25
if command.rFRA < 0:
command.rFRA = 0
return gripper.sendCommand()
def genCommand(req):
#Update the command according to request command
if req.command == 'activate':
command = outputMsg.SModel_robot_output()
command.rACT = 1
command.rGTO = 1
command.rSPA = 255
command.rFRA = 150
if req.command == 'reset':
command = outputMsg.SModel_robot_output()
command.rACT = 0
if req.command == 'close':
command.rPRA = 255
if req.command == 'open':
command.rPRA = 0
if req.command == 'basic':
command.rMOD = 0
if req.command == 'pinch':
command.rMOD = 1
if req.command == 'wide':
command.rMOD = 2
if req.command == 'scissor':
command.rMOD = 3
#If the command entered is a int, assign this value to rPRA
try:
command.rPRA = int(req.command)
if command.rPRA > 255:
command.rPRA = 255
if command.rPRA < 0:
command.rPRA = 0
except ValueError:
pass
if req.command == 'f':
command.rSPA += 25
if command.rSPA > 255:
command.rSPA = 255
if req.command == 'l':
command.rSPA -= 25
if command.rSPA < 0:
command.rSPA = 0
if req.command == 'i':
command.rFRA += 25
if command.rFRA > 255:
command.rFRA = 255
if req.command == 'd':
command.rFRA -= 25
if command.rFRA < 0:
command.rFRA = 0
pub = rospy.Publisher('SModelRobotOutput', outputMsg.SModel_robot_output)
pub.publish(command)
return gripperResponse(True)
def genCommand(req):
global command
hand = sys.argv[1]
update_joint_states(hand, req.command)
#Update the command according to request command
if req.command == 'activate':
command = outputMsg.SModel_robot_output()
command.rACT = 1
command.rGTO = 1
command.rSPA = 255
command.rFRA = 150 #255
if req.command == 'reset':
command = outputMsg.SModel_robot_output()
command.rACT = 0
if req.command == 'close':
command.rPRA = 255
if req.command == 'open':
command.rPRA = 0
if req.command == 'basic':
command.rMOD = 0
if req.command == 'pinch':
command.rMOD = 1
if req.command == 'wide':
command.rMOD = 2
if req.command == 'scissor':
command.rMOD = 3
#If the command entered is a int, assign this value to rPRA
try:
command.rPRA = int(req.command)
if command.rPRA > 255:
command.rPRA = 255
if command.rPRA < 0:
command.rPRA = 0
except ValueError:
pass
if req.command == 'f':
command.rSPA += 25
if command.rSPA > 255:
command.rSPA = 255
if req.command == 'l':
command.rSPA -= 25
if command.rSPA < 0:
command.rSPA = 0
if req.command == 'i':
command.rFRA += 25
if command.rFRA > 255:
command.rFRA = 255
if req.command == 'd':
command.rFRA -= 25
if command.rFRA < 0:
command.rFRA = 0
print "---------------------------------"
print req.command
print " "
print command
print "---------------------------------"
pub.publish(command)
return gripperResponse(True)
