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