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)