def __init__(self): self.larm = baxter_interface.Limb("left") self.rarm = baxter_interface.Limb("right") self.larm.set_command_timeout(1) self.rarm.set_command_timeout(1)
def main(): right_filename = 'right_joint_state' leftt_filename = 'left_joint_state' rf = open(right_filename, 'w') lf = open(leftt_filename, 'w') epilog = '' arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__, epilog=epilog) parser.parse_args(rospy.myargv()[1:]) print("Initializing node... ") rospy.init_node("rsdk_joint_position_keyboard") print("Getting robot state... ") rs = baxter_interface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled if (init_state == False): print ("Baxter must be enabled") return left = baxter_interface.Limb('left') right = baxter_interface.Limb('right') grip_left = baxter_interface.Gripper('left', CHECK_VERSION) grip_right = baxter_interface.Gripper('right', CHECK_VERSION) lj = left.joint_names() rj = right.joint_names() done = False print_mode = False i = 0; start_force = 0; max_force = 0; while not done and not rospy.is_shutdown(): c = getch() if c: if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Example finished.") elif c == 't' or c == 'T': print left.joint_efforts() print right.joint_efforts() elif c == 'a' or c == 'A': print left.joint_angles() print right.joint_angles() elif c == 'v' or c == 'V': print left.joint_velocities() print right.joint_velocities() elif c == 'e' or c == 'E': print left.endpoint_pose() #print right.endpoint_pose() elif c == 'f' or c == 'F': print left.endpoint_effort() print right.endpoint_effort() elif c == 'y' or c == 'Y': print left.endpoint_velocity() print right.endpoint_velocity()
def send_image(path): img = cv2.imread(path) msg = cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8") pub = rospy.Publisher('/robot/xdisplay', Image, queue_size=100, latch=True) pub.publish(msg) # Sleep to allow for image to be published. rospy.sleep(1) if __name__ == '__main__': # initialize the ROS node, enable robot rospy.init_node("Baxter_Move_Subscribers") rs = baxter_interface.RobotEnable() rs.enable() # create an instance of baxter_interface's Limb, Gripper and head classes right_limb = baxter_interface.Limb("right") left_limb = baxter_interface.Limb("left") right_gripper = baxter_interface.Gripper('right') left_gripper = baxter_interface.Gripper('left') head = baxter_interface.Head() # set limb and gripper parameters right_limb.set_joint_position_speed(0.6) #0-1.0 # right_gripper.set_velocity(100.0) #0-100.0 # right_gripper.calibrate() # left_limb.set_joint_position_speed(0.6) #0-1.0 left_gripper.set_velocity(100.0) #0-100.0 left_gripper.calibrate() # initialize current and goal pose of the robot
def main(): rospy.init_node("dmp_ability_test", anonymous=True) # this file is recoded by baxter # run this command to record data "rosrun baxter_examples joint_recorder.py -f path/file_name global data_path, train_len, tau, limb_name limb = baxter_interface.Limb(limb_name) train_set = pd.read_csv(data_path) #using pandas read data resample_t = np.linspace(train_set.values[0, 0], train_set.values[-1, 0], train_len) # resampling the time postion_x = np.interp(resample_t, train_set.values[:, 0], train_set.values[:, 1]) postion_y = np.interp(resample_t, train_set.values[:, 0], train_set.values[:, 2]) postion_z = np.interp(resample_t, train_set.values[:, 0], train_set.values[:, 3]) orientation_x = np.interp(resample_t, train_set.values[:, 0], train_set.values[:, 4]) orientation_y = np.interp(resample_t, train_set.values[:, 0], train_set.values[:, 5]) orientation_z = np.interp(resample_t, train_set.values[:, 0], train_set.values[:, 6]) orientation_w = np.interp(resample_t, train_set.values[:, 0], train_set.values[:, 7]) traj = [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] * train_len for i in range(train_len): traj[i] = [ postion_x[i], postion_y[i], postion_z[i], orientation_x[i], orientation_y[i], orientation_z[i], orientation_w[i] ] train_set_T = np.array([ np.array([ postion_x, postion_y, postion_z, orientation_x, orientation_y, orientation_z, orientation_w ]).T ]) param_w, base_function = train(train_set_T) # object_vision = get_base_camera_pose(flag = True) end_pose = limb.endpoint_pose() # get current angles endpoint_pose = [ end_pose['position'].x, end_pose['position'].y, end_pose['position'].z, end_pose['orientation'].x, end_pose['orientation'].y, end_pose['orientation'].z, end_pose['orientation'].w ] # ipdb.set_trace() start_point = traj[0] # start_point = endpoint_pose # print start_point # ending_point = object_vision ending_point = traj[-1] # print ending_point dmp = pydmps.dmp_discrete.DMPs_discrete(n_dmps=7, n_bfs=500, w=param_w) for i in range(7): dmp.y0[i] = start_point[i] #set the initial state dmp.goal[i] = ending_point[i] # set the ending goal y_track, dy_track, ddy_track = dmp.rollout(tau=tau) # ? time_dmp = np.linspace(0, train_set.values[-1, 0], train_len) dmp_data_w_time = np.column_stack((time_dmp, y_track)) # ipdb.set_trace() ####################################### For plotting #creat fig fig = plt.figure(1) ax = Axes3D(fig) plt.xlabel('X') plt.ylabel('Y') #plot traj fig ax.plot(postion_x, postion_y, postion_z, linewidth=2, alpha=0.3) #Plot plan fig ax.plot(y_track[:, 0], y_track[:, 1], y_track[:, 2], linewidth=2, alpha=0.3) #Plot plan fig #show the plot plt.draw() plt.show() # uncomment to plot return dmp_data_w_time, limb_name
def __init__(self, arm, distance, colours, board): # piece data self.playerPiece = 'X' self.playerPieceIndex = 1 # ( 1 or 2, 3 = board) self.robotPiece = 'R' self.robotPieceIndex = 2 # space detection self.board = board self.boardHeight = 0.2971 self.boardWidth = 0.410 self.storePointPix = True self.detectedAllCorners = False self.pixelPoints = [] self.board_tolerance = 0.00375 # board xyz spaces = self.board.getSpaces() self.boardZ = len(spaces) # levels self.boardY = len(spaces[0]) # rows self.boardX = len(spaces[0][0]) # columns # Background subtraction self.bgsubImgmsg = None self.imgmsg = None # Circle piece detection self.numCircleFrames = 1 self.spacePixelDist = 50.0 # arm ("left" or "right") self.limb = arm self.limb_interface = baxter_interface.Limb(self.limb) if arm == "left": self.other_limb = "right" else: self.other_limb = "left" self.other_limb_interface = baxter_interface.Limb(self.other_limb) # gripper ("left" or "right") self.gripper = baxter_interface.Gripper(arm) # object color dictionary data self.objects = colours # zeroed lists for pixel coordinates of objects self.x = np.zeros(shape=(len(colours)), dtype=int) self.y = np.zeros(shape=(len(colours)), dtype=int) # speeds self.normal = 0.8 self.slow = 0.1 # start positions self.start_pos_x = 0.50 # x = front back self.start_pos_y = 0.30 # y = left right self.start_pos_z = 0.15 # z = up down self.roll = -1.0 * math.pi # roll = horizontal self.pitch = -0.0 * math.pi # pitch = vertical self.yaw = 0.0 * math.pi # yaw = rotation self.pose = [self.start_pos_x, self.start_pos_y, self.start_pos_z, \ self.roll, self.pitch, self.yaw] # distances self.distance = distance #self.pixelHeightConst = 0.0022762 # constant for distance = 0.383, change for different heights!!! self.block_height = 0.04 self.block_pickup_height = self.distance - self.block_height self.block_grip_height = 0.102 self.block_tolerance = 0.005 # camera parameters (NB. other parameters in open_camera) # self.cam_calib = 0.0025 # original # meters per pixel at 1 meter self.cam_calib = 0.0022762 # constant for distance = 0.383, change for different heights!!! self.cam_x_offset = 0.045 # original camera gripper offset self.cam_y_offset = -0.015 #-0.01 #self.cam_x_offset = 0.045 # camera gripper offset #self.cam_y_offset = -0.02 self.width = 960 # Camera resolution self.height = 600 # Enable the actuators baxter_interface.RobotEnable().enable() # set speed as a ratio of maximum speed self.limb_interface.set_joint_position_speed(0.8) self.other_limb_interface.set_joint_position_speed(0.8) # calibrate the gripper self.gripper.calibrate() # set camera resolution self.config_camera(self.limb, self.width, self.height) # subscribe to required camera self.subscribe_to_camera(self.limb) # Display camera feed to face display self.displayCamera = False self.displayContours = False self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1) # move other arm out of harms way if arm == "left": self.baxter_ik_move("right", (0.25, -0.50, 0.2, math.pi, 0.0, 0.0)) else: self.baxter_ik_move("left", (0.25, 0.50, 0.2, math.pi, 0.0, 0.0))
def __init__(self): if not self.load_parameters(): sys.exit(1) self._limb = baxter_interface.Limb(self._arm) self._kin = baxter_kinematics(self._arm) self._planner = PathPlanner('{}_arm'.format(self._arm)) rospy.on_shutdown(self.shutdown) plan = self._planner.plan_to_joint_pos( np.array([-0.6, -0.4, -0.5, 0.6, -0.4, 1.1, -0.5])) self._planner.execute_plan(plan) rospy.sleep(5) ################################## Tensorflow bullshit if self._learning_bool: # I DON'T KNOW HOW THESE WORK #define placeholders observation_space = spaces.Box(low=-100, high=100, shape=(21, ), dtype=np.float32) action_space = spaces.Box(low=-50, high=50, shape=(56, ), dtype=np.float32) self._x_ph, self._u_ph = core.placeholders_from_spaces( observation_space, action_space) #define actor critic #TODO add in central way to accept arguments self._pi, self._logp, self._logp_pi, self._v = core.mlp_actor_critic( self._x_ph, self._u_ph, hidden_sizes=(128, 2), action_space=action_space) # POLY_ORDER = 2 # self._pi, self._logp, self._logp_pi, self._v = core.polynomial_actor_critic( # self._x_ph, self._u_ph, POLY_ORDER, action_space=action_space) #start up tensorflow graph var_counts = tuple(core.count_vars(scope) for scope in [u'pi']) print(u'\nYoyoyoyyoyo Number of parameters: \t pi: %d\n' % var_counts) self._tf_vars = [ v for v in tf.trainable_variables() if u'pi' in v.name ] self._num_tf_vars = sum( [np.prod(v.shape.as_list()) for v in self._tf_vars]) print("tf vars is of length: ", len(self._tf_vars)) print("total trainable vars: ", len(tf.trainable_variables())) self._sess = tf.Session() self._sess.run(tf.global_variables_initializer()) print("total trainable vars: ", len(tf.trainable_variables())) self._last_time = rospy.Time.now().to_sec() current_position = utils.get_joint_positions(self._limb).reshape( (7, 1)) current_velocity = utils.get_joint_velocities(self._limb).reshape( (7, 1)) self._last_x = np.vstack([current_position, current_velocity]) self._last_xbar = self._last_x if self._learning_bool: self._last_a = self._sess.run(self._pi, feed_dict={ self._x_ph: self.preprocess_state( self._last_x).reshape(1, -1) }) else: self._last_a = np.zeros((1, 56)) #################################### Set Tensorflow from saved params self._READ_PARAMS = self._is_test_set PARAM_INDEX = 201 ## This is the index of the learned parameters that you'll use (which epoch) if self._learning_bool: if self._READ_PARAMS: import dill # Put ABSOLUTE path to location of learned_params.pkl here, # then remove the NotImplementedError. (This will probably be the # same as the PREFIX variable in data_collector.py) # eg. PREFIX = "/home/cc/ee106a/fa19/staff/ee106a-taf/Desktop/data" PREFIX = "/home/cc/ee106b/sp20/staff/ee106b-laa/Desktop/data/read_params" # raise NotImplementedError lp = dill.load(open(PREFIX + "/learned_params.pkl", "rb")) print("Running training set using data from epoch number: " + str(PARAM_INDEX)) param_list = [] for param in lp[PARAM_INDEX][1]: p_msg = Parameters(param) param_list.append(p_msg) lp_msg = LearnedParameters(param_list) self._sess.run( tf.assign( tf.get_default_graph().get_tensor_by_name( 'pi/log_std:0'), tf.zeros((56, )))) self.params_callback(lp_msg) self._last_a = self._sess.run( self._pi, feed_dict={ self._x_ph: self.preprocess_state(self._last_x).reshape(1, -1) }) ##################################### Controller params self._A = np.vstack([ np.hstack([np.zeros((7, 7)), np.eye(7)]), np.hstack([np.zeros((7, 7)), np.zeros((7, 7))]) ]) self._B = np.vstack([np.zeros((7, 7)), np.eye(7)]) Q = 0.2 * np.diag([ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 ]) R = np.eye(7) def solve_lqr(A, B, Q, R): P = solve_continuous_are(A, B, Q, R) K = np.dot(np.dot(np.linalg.inv(R), B.T), P) return K self._K = solve_lqr(self._A, self._B, Q, R) # self._Kp = 6*np.diag(np.array([1, 1, 1.5, 1.5, 1, 1, 1])) # self._Kv = 5*np.diag(np.array([2, 2, 1, 1, 0.8, 0.3, 0.3])) # self._Kp = 9*np.diag(np.array([4, 6, 4, 8, 1, 5, 1])) # self._Kv = 5*np.diag(np.array([2, 3, 2, 4, 0.8, 1.5, 0.3])) # WORKS WELL FOR TRAINING self._Kp = 6 * np.diag(np.array([1, 1, 1, 1, 1, 1, 1])) self._Kv = 6 * np.diag(np.array([1, 1, 1, 1, 1, 1, 1])) # Tune down for testing except I didn't # self._Kp = 5*np.diag(np.array([1, 1, 1, 1, 1, 1, 1])) # self._Kv = 5*np.diag(np.array([1, 1, 1, 1, 1, 1, 1])) if not self.register_callbacks(): sys.exit(1)
import time import random import pdb import os from baxter_core_msgs.msg import JointCommand from baxter_pykdl import baxter_kinematics rospy.init_node('baxter_kinematics') #Ponemos RATE de mensaje rate = rospy.Rate( 500) #10000 104 es la primera que funciona bien - Seria 0.1 ms o 10kHz #Creamos objeto PUBLICADOR pub = rospy.Publisher('/robot/limb/left/joint_command', JointCommand, queue_size=10) limb = baxter_interface.Limb('left') min_KP = 0 max_KP = 100 min_KD = 0.01 max_KD = 1 min_KI = 1 max_KI = 10 def individual(): #Crea individuo a = [random.uniform(min_KP, max_KP) for i in [0, 1, 2, 3, 4, 5, 6]] b = [random.uniform(min_KD, max_KD) for i in [7, 8, 9, 10, 11, 12, 13]] c = [random.uniform(min_KI, max_KI) for i in [14, 15, 16, 17, 18, 19, 20]] return a + b + c
def main(): print("Welcome to Sliders") rospy.init_node('dev_joint_sliders') rate = rospy.Rate(300) relax = 2 # relax factor for thresholding goal joint_tau = baxter_interface.settings.JOINT_ANGLE_TOLERANCE * relax joy_in = Joy() joy_in_axes = [] joy_in_btns = [] def _on_joy(data, joy_store): joy_store.axes = list(data.axes) joy_store.buttons = list(data.buttons) print data.axes robot = baxter_interface.RobotEnable() left = baxter_interface.Limb('left') right = baxter_interface.Limb('right') joy_sub = rospy.Subscriber('/joy', Joy, _on_joy, joy_in) print("waiting for first /joy message....") def body_fun(): print ">>", print(joy_in_axes) baxter_dataflow.wait_for(lambda: len(joy_in.axes) > 0, rate=10, timeout=5.0, body=body_fun) joint_maxs = [1.701, 1.047, 3.054, 2.618, 3.059, 2.094, 3.059] joint_mins = [-1.701, -2.147, -3.054, -0.050, -3.059, -1.571, -3.059] inc_major = [0.1] * 7 inc_minor = [0.01] * 7 half_range = [1.0] * 7 mid_major = [0.0] * 7 flip_left = [-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0] flip_right = [1.0, -1.0, 1.0, -1.0, 1.0, -1.0, 1.0] for j in range(7): inc_major[j] = (joint_maxs[j] - joint_mins[j]) / 128.0 inc_minor[j] = inc_major[j] / 128.0 half_range[j] = (joint_maxs[j] - joint_mins[j]) / 2.0 mid_major[j] = joint_mins[j] + half_range[j] def in_out(in_axes, j_max, j_min, flip, a_max=1.0, a_min=-1.0): pass def inc_in(idx, in_major, in_minor, flip): j_out = mid_major[idx] + flip * (half_range[idx] * in_major + inc_major[idx] * in_minor) # base = joint_maxs[idx] if flip < 0 else joint_mins[idx] # j_out = base + flip * (inc_major[idx] * in_major # + inc_minor[idx] * in_minor) return j_out idx_axes_major = [0, 1, 2, 3, 4, 5, 6] idx_axes_minor = [8, 9, 10, 11, 12, 13, 14] left_cmd = [0.0] * 7 right_cmd = [0.0] * 7 left.set_joint_position_speed(0.3) right.set_joint_position_speed(0.3) left.move_to_neutral() right.move_to_neutral() modes = ['mirror', 'left', 'right'] mode = 0 print("Sliding...") while not rospy.is_shutdown(): joy_in_buttons = joy_in.buttons joy_in_axes = joy_in.axes for j in range(7): left_cmd[j] = inc_in(j, joy_in_axes[idx_axes_major[j]], joy_in_axes[idx_axes_minor[j]], flip_left[j]) right_cmd[j] = inc_in(j, joy_in_axes[idx_axes_major[j]], joy_in_axes[idx_axes_minor[j]], flip_right[j]) print(left_cmd) print(right_cmd) left.set_joint_positions(dict(zip(left.joint_names(), left_cmd))) right.set_joint_positions(dict(zip(right.joint_names(), right_cmd))) # right.move_to_joint_positions(dict(zip(right.joint_names(), right_cmd)), timeout=0.1) rate.sleep()
def map_joystick(joystick): """ Maps joystick input to joint position commands. @param joystick: an instance of a Joystick """ left = baxter_interface.Limb('left') right = baxter_interface.Limb('right') grip_left = baxter_interface.Gripper('left', CHECK_VERSION) grip_right = baxter_interface.Gripper('right', CHECK_VERSION) lcmd = {} rcmd = {} #available joints lj = left.joint_names() rj = right.joint_names() #abbreviations jhi = lambda s: joystick.stick_value(s) > 0 jlo = lambda s: joystick.stick_value(s) < 0 bdn = joystick.button_down bup = joystick.button_up def print_help(bindings_list): print("Press Ctrl-C to quit.") for bindings in bindings_list: for (test, _cmd, doc) in bindings: if callable(doc): doc = doc() print("%s: %s" % (str(test[1][0]), doc)) bindings_list = [] bindings = ( ((bdn, ['rightTrigger']), (grip_left.close, []), "left gripper close"), ((bup, ['rightTrigger']), (grip_left.open, []), "left gripper open"), ((bdn, ['leftTrigger']), (grip_right.close, []), "right gripper close"), ((bup, ['leftTrigger']), (grip_right.open, []), "right gripper open"), ((jlo, ['leftStickHorz']), (set_j, [rcmd, right, rj, 0, 0.1]), lambda: "right inc " + rj[0]), ((jhi, ['leftStickHorz']), (set_j, [rcmd, right, rj, 0, -0.1]), lambda: "right dec " + rj[0]), ((jlo, ['rightStickHorz']), (set_j, [lcmd, left, lj, 0, 0.1]), lambda: "left inc " + lj[0]), ((jhi, ['rightStickHorz']), (set_j, [lcmd, left, lj, 0, -0.1]), lambda: "left dec " + lj[0]), ((jlo, ['leftStickVert']), (set_j, [rcmd, right, rj, 1, 0.1]), lambda: "right inc " + rj[1]), ((jhi, ['leftStickVert']), (set_j, [rcmd, right, rj, 1, -0.1]), lambda: "right dec " + rj[1]), ((jlo, ['rightStickVert']), (set_j, [lcmd, left, lj, 1, 0.1]), lambda: "left inc " + lj[1]), ((jhi, ['rightStickVert']), (set_j, [lcmd, left, lj, 1, -0.1]), lambda: "left dec " + lj[1]), ((bdn, ['rightBumper']), (rotate, [lj]), "left: cycle joint"), ((bdn, ['leftBumper']), (rotate, [rj]), "right: cycle joint"), ((bdn, ['btnRight']), (grip_left.calibrate, []), "left calibrate"), ((bdn, ['btnLeft']), (grip_right.calibrate, []), "right calibrate"), ((bdn, ['function1']), (print_help, [bindings_list]), "help"), ((bdn, ['function2']), (print_help, [bindings_list]), "help"), ) bindings_list.append(bindings) rate = rospy.Rate(100) print_help(bindings_list) print("Press Ctrl-C to stop. ") while not rospy.is_shutdown(): for (test, cmd, doc) in bindings: if test[0](*test[1]): cmd[0](*cmd[1]) if callable(doc): print(doc()) else: print(doc) if len(lcmd): left.set_joint_positions(lcmd) lcmd.clear() if len(rcmd): right.set_joint_positions(rcmd) rcmd.clear() rate.sleep() return False
#!/usr/bin/env python import rospy import baxter_interface rospy.init_node('ArmInit') rospy.loginfo("Started...") leftLimb = baxter_interface.Limb('left') rightLimb = baxter_interface.Limb('right') leftPoint = { 'left_s0': 0.5989, 'left_s1': 0.213, 'left_e0': -2.294, 'left_e1': 1.868, 'left_w0': -2.11, 'left_w1': -1.4734, 'left_w2': 3.289 } rightPoint = { 'right_s0': -0.7497331092224122, 'right_s1': -0.14227671791381838, 'right_w0': 2.49501974864502, 'right_w1': -1.0657331511657715, 'right_w2': 2.489, 'right_e0': 1.974616766949463, 'right_e1': 1.8411604385559084 } leftLimb.move_to_joint_positions(leftPoint) rightLimb.move_to_joint_positions(rightPoint)
import Queue as Q from positionControl import * from taskFunctions import * from std_msgs.msg import String from baxter_core_msgs.msg import CollisionDetectionState rospy.init_node('speechControl') # Global Variables rawCommand = "" collisionState = False lLimb = baxter.Limb('left') rLimb = baxter.Limb('right') lGripper = baxter.Gripper('left') rGripper = baxter.Gripper('right') # Text to speech engine t2s = pyttsx.init() voices = t2s.getProperty('voices') t2s.setProperty('voice', 'english') """ for voice in voices: print(voice.id) """ t2s.setProperty('rate', 150) ################## Definitions #####################
def __init__(self, argv, configFileName=None, ikseedsFileName=None, roadmapLeft=None, roadmapRight=None, leftElectricGripper=False, rightElectricGripper=False): self._lock = threading.RLock() moveit_commander.roscpp_initialize(argv) self._moveGroupPid = None # wait for move_group action to become available to avoid timeout # move_group_client = actionlib.SimpleActionClient('/move_group', MoveGroupAction) # rospy.loginfo('[apc_grasping/MoveItInterface]: waiting for move_group action server') # move_group_client.wait_for_server() self._robotCommander = None self._leftArmGroup = None self._moveitScene = None self._rightArmGroup = None self._preempted = False self._tfListener = utils.TFCache.TFCache() self._objectsInScene = [] moveGroupAvailable = False self._configurationStack = {'left_arm': [], 'right_arm': []} # check for move group availability (the ugly way....) while not moveGroupAvailable and not rospy.is_shutdown(): try: self.checkMoveGroup(bResetScene=False) moveGroupAvailable = True except MoveItMissingException: rospy.logwarn('Waiting for move_group') self.sleep(0.5) self._co_publisher = rospy.Publisher('/collision_object', CollisionObject, queue_size=1) self._co_attach_publisher = rospy.Publisher( '/attached_collision_object', AttachedCollisionObject, queue_size=1) self._leftGlobalPlanner = utils.APCGlobalPathPlanner.APCGlobalPathPlanner( 'left_arm', self._leftArmGroup, self.checkPreemption) self._rightGlobalPlanner = utils.APCGlobalPathPlanner.APCGlobalPathPlanner( 'right_arm', self._rightArmGroup, self.checkPreemption) self._armSwitchControllerService = {} rospy.wait_for_service('/apc/hacks/set_control_mode_left') self._armSwitchControllerService['left_arm'] = rospy.ServiceProxy( '/apc/hacks/set_control_mode_left', ControlMode) rospy.wait_for_service('/apc/hacks/set_control_mode_right') self._armSwitchControllerService['right_arm'] = rospy.ServiceProxy( '/apc/hacks/set_control_mode_right', ControlMode) rospy.wait_for_service('/pause_joint_trajectory_server_left') rospy.wait_for_service('/pause_joint_trajectory_server_right') self._pause_joint_trajectory_service_left = rospy.ServiceProxy( 'pause_joint_trajectory_server_left', Empty) self._pause_joint_trajectory_service_right = rospy.ServiceProxy( 'pause_joint_trajectory_server_right', Empty) # self._tfListener = tf.TransformListener() self._leftGripper = APCBaxterGripper(baxter_interface.Gripper('left'), electric=leftElectricGripper) self._rightGripper = APCBaxterGripper( baxter_interface.Gripper('right'), electric=rightElectricGripper) self._leftArm = baxter_interface.Limb('left') self._rightArm = baxter_interface.Limb('right') self._head = baxter_interface.Head() self._namedConfigurations = {} if roadmapLeft is not None: rospy.loginfo('Loading roadmap for left arm from path ' + roadmapLeft) rospy.loginfo('Note, this may take a while') self._leftGlobalPlanner.readRoadmap(roadmapLeft) rospy.loginfo('Roadmap successfully loaded!') if roadmapRight is not None: rospy.loginfo('Loading roadmap for right arm from path ' + roadmapRight) rospy.loginfo('Note, this may take a while') self._rightGlobalPlanner.readRoadmap(roadmapRight) rospy.loginfo('Roadmap successfully loaded!') self._seedIks = {} if ikseedsFileName is not None: try: ikseedFile = open(ikseedsFileName, 'r') self._seedIks = yaml.load(ikseedFile) ikseedFile.close() except IOError as err: rospy.logerr('Could not load ik seeds file: ' + repr(err)) if 'left_arm' not in self._seedIks or self._seedIks['left_arm'] is None: self._seedIks['left_arm'] = {} if 'right_arm' not in self._seedIks or self._seedIks[ 'right_arm'] is None: self._seedIks['right_arm'] = {} if configFileName is not None: try: configFile = open(configFileName, 'r') self._namedConfigurations = yaml.load(configFile) configFile.close() except IOError as err: rospy.logerr('Could not load configurations from file ' + configFileName) if 'left_arm' not in self._namedConfigurations: self._namedConfigurations['left_arm'] = {} if 'right_arm' not in self._namedConfigurations: self._namedConfigurations['right_arm'] = {} rospy.loginfo('MoveItInterface created.')
def main(): # If you want to debug, uncomment the next line. ipdb.set_trace() # Initialize node rospy.init_node('example3_kdl') # Call routine to enable the robot rs = enable_Baxter() ## Params loop = rospy.Rate(rate) dp = zeros((3, 1)) # Create Limb Objects rLimb = baxter_interface.Limb('right') #lLimb=baxter_interface.Limb('left') # Create Kinematic Objects rKin = baxter_kinematics('right') #lKin = baxter_kinematics('left') # Get Joint names #jNamesR_ordrered=rLimb.joint_names() jNamesR = [ 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2', 'right_e0', 'right_e1' ] # Set Vector to some goal to exploit redundancy. # A. Vector pointing to mid-range of joint angles if dq_to_ctr_mode: upper_limits = [0.890, 1.047, 3.028, 2.618, 3.059, 2.094, 3.059] # s0s1e0e1w0w1w2 lower_limits = [-2.461, -2.147, -3.028, -0.052, -3.059, -1.571, -3.059] mid_range = [(upper_limits[i] - lower_limits[i]) / 2 for i in range(7)] q_red = matrix([ upper_limits[i] - mid_range[i] for i in range(7) ]).T # This is in effect a q_dot, a velocity of joint angles. else: # B. Vector with a simple q_red = matrix( '0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0' ).T # Select the joint you want to update in this 7x1 col vector for i in range( len(q_red) ): # Identify the joint that is non-zero and save that index. if q_red[i, 0] != 0.0: idx = i # Matrix indeces start from zero # Set a reference pose: [x,y,z,vx,vy,vz,w]. This one is near the home position of arm. # Home joint angle position for baxter_world_home.launch is approximately: [0.58,-0.19,0.23,-0.11,0.99,0.01,0.02] ref_pose = { 'position': [0.70, -0.2, 0.23], 'orientation': [-0.12, 0.99, 0.01, 0.01] } ref_pos = matrix(ref_pose['position']).T #ref_rot=matrix(ref_pose['orientation']) print 'The reference pos is: ' print(ref_pos) # Right Arm while not rospy.is_shutdown(): print 'example3_kdl for the right arm...' # 1. Get the current angles and jacobian as numpy matrices rAngles = rLimb.joint_angles() # returns as s0s1w0w1w2e0e1 rAnglesM = matrix(rAngles.values()).T jac_n = rKin.jacobian( ) # wrt end-effector frame. TODO: change to world frame. # 2. Compute the forward Kinematics using the Jacobian: del_p=J(q)del_q curr_pos = rLimb.endpoint_pose() curr_pos = matrix(curr_pos['position']).T print 'The current position is:\t\t The reference positions is:' for i in range(len(curr_pos)): print repr(curr_pos[i, 0]).rjust(10), repr(ref_pos[i, 0]).rjust(40) # Position Mode if pos_mode: # 3. Compute the error between reference and current positions del_p_error = Kp * (ref_pos - curr_pos[0:3]) # 4. Get a scalar distance (vector norm) to more easily interpret the error dp_norm = norm(del_p_error) print 'the dpnorm is %f' % dp_norm # 5. Extract the translation Jacobian jTrans = jac[0:3, 0:7] # when slicing go one dim past end # 6. Compute the dq using dp_error del_q = (pinv(jTrans)) * del_p_error # in terms of dp change # 7. Add this dq to current joint Angles ref_anglesM = del_q + rAnglesM ref_angles = ref_anglesM.ravel().tolist()[ 0] # change back to list.[0] b/c it returns nested list. ref_angles = dict(zip(jNamesR, ref_angles)) # 8. Move the Limb wout working with the null space #rLimb.move_to_joint_positions(ref_angles) # need to check that indeces are in the right order rLimb.set_joint_positions(ref_angles) # Velocity Mode else: # 3. Compute the error between reference and current positions del_p_error = Kv * (ref_pos - curr_pos[0:3]) for i in range(len(del_p_error)): dp[i] = del_p_error[i] / dt # 4. Get a scalar distance (vector norm) to more easily interpret the error dp_norm = norm(del_p_error) print 'the dpnorm is %f' % dp_norm # 5. Extract the translation Jacobian jTrans = jac[0:3, 0:7] # when slicing go one dim past end # 6. Compute the dq using dp_error #del_q=(pinv(jTrans))*del_p_error # in terms of dp change dq = (pinv(jTrans)) * dp # in terms of velocity # 7. Add this dq to current joint Angles ref_anglesM = (dt * dq) + rAnglesM # Covert from speed to distance ref_angles = ref_anglesM.ravel().tolist()[ 0] # change back to list.[0] b/c it returns nested list. ref_angles = dict(zip(jNamesR, ref_angles)) # 8. Move the Limb wout working with the null space #rLimb.move_to_joint_positions(ref_angles) # need to check that indeces are in the right order rLimb.set_joint_positions(ref_angles) if null_space_comp: # 9. Compute the kernel (null space) for the translational Jacobian of Baxter (3x6) and the pseudo-inverse jNull = matrix(null(jTrans)) jNull_inv = matrix(pinv(jNull)) # Test to make sure that the output of the translational Jacobian(3,7) times the kernel(7,4) is the zero matrix(3,4): z = jTrans * jNull print '\nTest to make sure that the output of the translational Jacobian(3,7) times the kernel(7,4), is the zero matrix(3,4):\njTrans*jNull is: ' print z # 10. Compute direction of motion. # If dqt_to_ctr, then compute error from center of joint range: desired - actual # Scale the error so that it is appropriate for the Jacobian Null Space. if dq_to_ctr_mode: dq_ref = Kq * (q_red - ref_anglesM) print '\nCenter Angles\t\t Current Angles\t\t\t Error' for i in range(len(q_red)): print repr(q_red[i, 0]).rjust(7), repr( ref_anglesM[i, 0]).rjust(33), repr(dq_ref[i, 0]).rjust(29) else: dq_ref = q_red print '\nThe error vector in radians is:' print(dq_ref) # Compute the norm dq_ref_norm = norm(dq_ref) print 'And, the norm of our error reference is: ' print(dq_ref_norm) # 11. Project this error to the nullspace via NN*dq_to_ctr null_proj = jNull * jNull_inv dq_null = null_proj * dq_ref print '\nThen, this error vector, projected unto the nullspace dq_null is:' print dq_null print '' # 12. Scale dq_null. # If you are only changing a single joint, the projection will modify that dimensions of that joint. # We can do a scaling mechanism to reset that specific joint value to our original goal. if single_joint_update_mode: for i in range(len(dq_null)): dq_null[i, 0] = (dq_null[i, 0] / dq_null[idx, 0]) * q_red[ idx, 0] # for loop index starts at 1 but the matrix index at 0 # 13. If norm is less than 0.00001 then add print 'If we multipy dq_null x Jacobian and compute the norm, it should be zero...' zero = norm(jTrans * dq_null) print(zero) # 14. Only move the joints if the computed dq_null in fact gives a small (near zero) output. Otherwise # do not move the joint angles. This results in a much more stable motion. if zero < 0.00001: ref_null_anglesM = ref_anglesM + dq_null # 12. Convert back to list and move ref_null_angles = ref_null_anglesM.ravel().tolist()[ 0] # change back to list.[0] b/c it returns nested list. ref_null_angles = dict(zip(jNamesR, ref_null_angles)) #rLimb.set_joint_positions(ref_null_angles) rLimb.move_to_joint_positions(ref_null_angles) # Set the loop speed loop.sleep() # Clear the screen for easier reading os.system('clear') # Set rospy to execute a shutdown function when exiting rospy.on_shutdown(shutdown) return rs
def main(): rospy.init_node('kinect_test') vive_data = [] rate = rospy.Rate(30) rospy.Subscriber("unity_locations", String, vive_callback) left = baxter_interface.Limb('left') right = baxter_interface.Limb('right') left_kin = baxter_kinematics('left') right_kin = baxter_kinematics('right') print('Program Starting! 10 Seconds to move to initial pose') rospy.sleep(6.0) print('Pose Acquired, Demonstrator must hold this position for now!!!!') for i in vive: vive_data.append(float(i)) human_data = np.array(vive_data) print "human_data" print human_data c1 = human_data[0:7] c2 = human_data[7:14] head = human_data[14:21] c1[0] = c1[0] - head[0] c1[1] = c1[1] - head[1] c1[2] = c1[2] - head[2] c2[0] = c2[0] - head[0] c2[1] = c2[1] - head[1] c2[2] = c2[2] - head[2] human_data = np.concatenate((c1, c2)) human_data = np.reshape(human_data, (1,14)) print human_data right_json_file = open('right_nn_without_head.json', 'r') right_loaded_model_json = right_json_file.read() right_json_file.close() right_joint_nn_model = model_from_json(right_loaded_model_json) # load weights into new model right_joint_nn_model.load_weights("right_nn_without_head_wt.h5") right_joint_nn_model.compile(loss = 'mean_squared_error', optimizer='sgd') right_predicted_joints = right_joint_nn_model.predict(human_data)[0] print right_predicted_joints left_neutral_position = {'left_s0':0.948767, 'left_s1':0.901981, 'left_e0':-1.456131, 'left_e1':0.541495, 'left_w0':0.887408, 'left_w1':0.488189, 'left_w2':-2.937190} right_neutral_position = {'right_s0':-1.039272, 'right_s1':0.868233, 'right_e0':1.078005, 'right_e1':0.537660, 'right_w0':-3.045335, 'right_w1':-0.272282, 'right_w2':-1.091811} left.move_to_joint_positions(left_neutral_position) print "left completed" right.move_to_joint_positions(right_neutral_position) right.move_to_joint_positions({'right_s0': right_predicted_joints[0],'right_s1': right_predicted_joints[1],'right_e0': right_predicted_joints[2],'right_e1': right_predicted_joints[3],'right_w0': right_predicted_joints[4],'right_w1':right_predicted_joints[5],'right_w2': right_predicted_joints[6]}) #print 'Want Robot Ready to Move?[y/n]', #userInput = sys.stdin.readline().strip() #if(userInput == 'y'): #left.move_to_joint_positions(left_neutral_position) #print "left completed" #right.move_to_joint_positions(right_neutral_position) #right.move_to_joint_positions({'right_s0': right_predicted_joints[0],'right_s1': right_predicted_joints[1],'right_e0': right_predicted_joints[2],'right_e1': right_predicted_joints[3],'right_w0': right_predicted_joints[4],'right_w1':right_predicted_joints[5],'right_w2': right_predicted_joints[6]}) #else: #print("Program exiting...") #return #print('Attempt real time tracking?[y/n]') #if(userInput != 'y'): # print("Program exiting...") # return pub_right = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10) pub_left = rospy.Publisher('/robot/limb/left/joint_command', JointCommand, queue_size=10) print('Realtime Tracking Starting! Demonstrator can move very slowly now') while not rospy.is_shutdown(): vive_data = [] for i in vive: vive_data.append(float(i)) human_data = np.array(vive_data) c1 = human_data[0:7] c2 = human_data[7:14] head = human_data[14:21] c1[0] = c1[0] - head[0] c1[1] = c1[1] - head[1] c1[2] = c1[2] - head[2] c2[0] = c2[0] - head[0] c2[1] = c2[1] - head[1] c2[2] = c2[2] - head[2] human_data = np.concatenate((c1, c2)) human_data = np.reshape(human_data, (1,14)) print human_data right_predicted_joints = right_joint_nn_model.predict(human_data)[0] #right_end_effector = right_kin.forward_position_kinematics({'right_s0': right_predicted_joints[0],'right_s1': right_predicted_joints[1],'right_e0': right_predicted_joints[2],'right_e1': right_predicted_joints[3],'right_w0': right_predicted_joints[4],'right_w1':right_predicted_joints[5],'right_w2': right_predicted_joints[6]}) #print "end effector:" #print right_end_effector #with open("test_end_effector_vive.csv", 'a') as myfile: # wr = csv.writer(myfile, quoting=csv.QUOTE_ALL) # wr.writerow(np.concatenate((np.array(right_end_effector), human_data[0]))) print "predicted joints:" print right_predicted_joints #right.move_to_joint_positions({'right_s0': right_predicted_joints[0],'right_s1': right_predicted_joints[1],'right_e0': right_predicted_joints[2],'right_e1': right_predicted_joints[3],'right_w0': right_predicted_joints[4],'right_w1':right_predicted_joints[5],'right_w2': right_predicted_joints[6]}) command_msg = JointCommand() command_msg.mode = JointCommand.POSITION_MODE command_msg.command = [right_predicted_joints[i] for i in xrange(0, 6)] command_msg.names = ['right_s0', 'right_s1','right_e0','right_e1','right_w0','right_w1', 'right_w2'] pub_right.publish(command_msg) rate.sleep()
def Step5(q1, q2, q3, q4, s): limb = baxter_interface.Limb('right') angles = limb.joint_angles() def ik_test_Luky(limb, poza): ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') poses = { 'right': PoseStamped( header=hdr, pose=Pose( position=Point( x=poza['position'].x + 0.2, y=poza['position'].y + (0.2 * tan(s)), #-0.25, #-0.2837, z=poza['position'].z), orientation=Quaternion( x=q1, y=q2, z=q3, w=q4, ), ), ), } ikreq.pose_stamp.append(poses[limb]) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e, )) return 1 # Check if result valid, and type of seed ultimately used to get solution # convert rospy's string representation of uint8[]'s to int's resp_seeds = struct.unpack('<%dB' % len(resp.result_type), resp.result_type) if (resp_seeds[0] != resp.RESULT_INVALID): seed_str = { ikreq.SEED_USER: '******', ikreq.SEED_CURRENT: 'Current Joint Angles', ikreq.SEED_NS_MAP: 'Nullspace Setpoints', }.get(resp_seeds[0], 'None') print("\nSUCCESS - Valid Joint Solution Found from Seed Type: %s" % (seed_str, )) # Format solution into Limb API-compatible dictionary limb_joints = dict( zip(resp.joints[0].name, resp.joints[0].position)) #print "\nIK Joint Solution:\n", limb_joints print "------------------" #print "Response Message:\n", resp else: print("INVALID POSE - No Valid Joint Solution Found.") return limb_joints
def main(): # If you want to debug, uncomment the next line. # pdb.set_trace() # Initialize node rospy.init_node('example_baxter_kins_right') # Call routine to enable the robot rs = enable_Baxter() # Set a ROS Rate for the while loop at 1Hz loop = rospy.Rate(5) # Create Limb Objects rLimb = baxter_interface.Limb('right') lLimb = baxter_interface.Limb('left') # Create Kinematic Objects rKin = baxter_kinematics('right') lKin = baxter_kinematics('left') # Angles and Kinematics print 'Printing joing angles from baxter_interface, fkine, ikine. Please move baxter\'s joints' # Right Arm while not rospy.is_shutdown(): print 'Right Arm'.center(35, '*') print 'Joint Angles from Baxter Interface' rAngles = rLimb.joint_angles() # Angles print 's0\ts1\te0\te1\tw0\tw1\tw2' # Angle sequence print(round(rAngles['right_s0'], 4), round(rAngles['right_s1'], 4), round(rAngles['right_e0'], 4), round(rAngles['right_e1'], 4), round(rAngles['right_w0'], 4), round(rAngles['right_w1'], 4), round(rAngles['right_w2'], 4)) print '' # Empty line print 'Pose from FKins...' pose = rKin.forward_position_kinematics() # Fkine print( 'x: ', round(pose[0], 2), 'y: ', round(pose[1], 2), 'z: ', round(pose[2], 2), 'vx: ', round(pose[3], 2), 'vy: ', round(pose[4], 2), 'vz: ', round(pose[5], 2), 'w: ', round(pose[6], 2), ) print '' # Get End pose, without orientation print 'Cartesian End-Effector Pose' rPose = rLimb.endpoint_pose() print( 'x: ', round(rPose['position'][0], 2), 'y: ', round(rPose['position'][1], 2), 'z: ', round(rPose['position'][2], 2), 'vx: ', round(rPose['orientation'][0], 2), 'vy: ', round(rPose['orientation'][1], 2), 'vz: ', round(rPose['orientation'][2], 2), 'w: ', round(rPose['orientation'][3], 2), ) print '' #print 'The RPY is: ', quat_to_angle(Quaternion(*orientation)) print 'Joint Angles from IKins...' rIKin = rKin.inverse_kinematics(rPose['position'], rPose['orientation']) print round(rIKin[0], 4), round(rIKin[1], 4), round(rIKin[2], 4), round( rIKin[3], 4), round(rIKin[4], 4), round(rIKin[5], 4), round(rIKin[6], 4) print '' # Set the loop speed loop.sleep() # Clear the screen for easier reading os.system('clear') # Set rospy to execute a shutdown function when exiting rospy.on_shutdown(shutdown) return rs
def robot_setup(self): baxter_interface.RobotEnable().enable() self.robot = baxter_interface.Limb('right') self.robot.set_joint_position_speed(0.5) self.talk = actionlib.SimpleActionClient('/speak', maryttsAction)
feedback.feedback = 1 result = BaxterActionResult() result.result = 1 print "Right hand moving !" angles = dict() wave_1 = { 'right_s0': goal.coordinates[0], 'right_s1': goal.coordinates[1], 'right_e0': goal.coordinates[2], 'right_e1': goal.coordinates[3], 'right_w0': goal.coordinates[4], 'right_w1': goal.coordinates[5], 'right_w2': goal.coordinates[6] } limbR.move_to_joint_positions(wave_1, 15, goal.precision) server.publish_feedback(feedback) server.set_succeeded(result) rospy.sleep(0.1) limbR = baxter_interface.Limb('right') server = actionlib.SimpleActionServer('/Rhand_server', BaxterActionAction, callback, False) server.start() print("right hand ready") rospy.spin()
math.sqrt(crossVector[2]**2)) crossVector_norm[0] = crossVector[0] / magnitude crossVector_norm[1] = crossVector[1] / magnitude crossVector_norm[2] = crossVector[2] / magnitude print 'normalized cross vector', crossVector_norm getNormalVector2() print("MIM tutorial: forward kinematics.") # Initialising ROS node rospy.init_node("SSE_forward_kinematics") ###################### INSERT YOUR CODE HERE # Create a "Limb" instance called "left_arm" linked to Baxter's left limb left_arm = baxter_interface.Limb("left") # Create a "pose" variable which holds the output of endpoint_pose() pose = left_arm.endpoint_pose() angles = left_arm.joint_angles() print angles lcmd = { 'left_w0': -0.381669966846, 'left_w1': 0.22169442983, 'left_w2': -0.98, 'left_e0': -0.751267090009, 'left_e1': 1.02009722278, 'left_s0': 0.335558296967, 'left_s1': -0.214757310059 }
robot = moveit_commander.RobotCommander() # Add in objects p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = 0.8 p.pose.position.y = 0.025 p.pose.position.z = -0.6 scene.add_box("table", p, (0.75, 1.25, 0.68)) traj_controller = Trajectory_Controller() ikright = Inverse_Kinematics(limb="right") right_arm = baxter_interface.Limb('right') left_arm = baxter_interface.Limb('left') while not rospy.is_shutdown(): rospy.wait_for_service('joint_values') try: joint_service_proxy = rospy.ServiceProxy('joint_values', JointValues) joint_resp = joint_service_proxy() if joint_resp.new_values: if "joints" in joint_resp.val_type: joints = dict( zip(joint_resp.joint_names, joint_resp.joint_values)) try: both_arms_group.set_joint_value_target(joints) traj_controller.push(both_arms_group.plan())
class MoveArms(SmokeTest): """ Move both arms to numerous joint positions. """ def __init__(self, name='MoveArms'): super(MoveArms, self).__init__(name) def start_test(self): """Runs MoveArms Smoke Test. """ def move_thread(limb, angle, queue, timeout=15.0): """ Threaded joint movement allowing for simultaneous joint moves. """ try: limb.move_to_joint_positions(angle, timeout) queue.put(None) except Exception, exception: queue.put(traceback.format_exc()) queue.put(exception) try: print "Enabling robot..." self._rs.enable() print "Test: Create Limb Instances" right = baxter_interface.Limb('right') left = baxter_interface.Limb('left') left_queue = Queue.Queue() right_queue = Queue.Queue() # Max Joint Range (s0, s1, e0, e1, w0, w1, w2) # ( 1.701, 1.047, 3.054, 2.618, 3.059, 2.094, 3.059) # Min Joint Range (e0, e1, s0, s1, w0, w1, w2) # (-1.701, -2.147, -3.054, -0.050, -3.059, -1.571, -3.059) joint_moves = ( [0.0, -0.55, 0.0, 0.75, 0.0, 1.26, 0.0], [0.5, -0.8, 2.8, 0.15, 0.0, 1.9, 2.8], [-0.1, -0.8, -1.0, 2.5, 0.0, -1.4, -2.8], [0.0, -0.55, 0.0, 0.75, 0.0, 1.26, 0.0], ) for move in joint_moves: print "Test: Moving to Joint Positions: ", print ", ".join("%.2f" % x for x in move) left_thread = threading.Thread( target=move_thread, args=(left, dict(zip(left.joint_names(), move)), left_queue)) right_thread = threading.Thread( target=move_thread, args=(right, dict(zip(right.joint_names(), move)), right_queue)) left_thread.daemon = True right_thread.daemon = True left_thread.start() right_thread.start() baxter_dataflow.wait_for( lambda: not (left_thread.is_alive() or right_thread. is_alive()), timeout=20.0, timeout_msg=("Timeout while waiting for arm move threads" " to finish"), rate=10, ) left_thread.join() right_thread.join() result = left_queue.get() if not result is None: raise left_queue.get() result = right_queue.get() if not result is None: raise right_queue.get() rospy.sleep(1.0) print "Disabling robot..." self._rs.disable() self.result[0] = True rospy.sleep(5.0) except Exception: self.return_failure(traceback.format_exc())
Range, ) from trac_ik_python.trac_ik import IK from baxter_interface import CHECK_VERSION import tf2_ros import tf2_geometry_msgs import random as rd rospy.init_node("marker_ik_example_movement", disable_signals=True) global tf_buffer tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0)) #tf buffer length #set limb speed right = baxter_interface.Limb('right') right.set_joint_position_speed(1.0) left = baxter_interface.Limb('left') left.set_joint_position_speed(1.0) #prev time for marker callback global prev_time prev_time = 0 #range array for laser range finder global range_array range_array = collections.deque(5 * [(0.0, 0.0)], 5) #endpoint array of arm positions global endpoint_array endpoint_array = [] #array of markers global marker_array
def main(): rospy.init_node('execute') rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION) rs.enable() # retrieve images global current_image def update_image(msg): global current_image current_image = PIL_Image.frombytes('RGBA', (msg.width, msg.height), msg.data) # print msg.width, msg.height, msg.is_bigendian, msg.step, msg.encoding rospy.Subscriber('/cameras/left_hand_camera/image', Image, update_image) # model width = 128 checkpoint_dir = 'checkpoints-dev-rgb-4-max' grasp_class_prediction, logit, grasp_image_ph, keep_prob_ph = build_model( width) saver = tf.train.Saver(max_to_keep=5, keep_checkpoint_every_n_hours=1) arm = baxter_interface.Limb('left') arm.move_to_neutral() gripper = baxter_interface.Gripper('left') gripper.calibrate() # grasp crop crop_center_x = 330 crop_center_y = 160 grasp_class_threashold = 0.5 scale = 1.0 crop_width = width * scale crop_box = (crop_center_x - crop_width / 2, crop_center_y - crop_width / 2, crop_center_x + crop_width / 2, crop_center_y + crop_width / 2) # grasp workspace x0 = 0.81 y0 = 0.25 delta = 0.04 initial_z = 0.1 bound_z = -0.165 grasp_class_threashold = 0.5 pub = rospy.Publisher('/robot/xdisplay', Image, queue_size=1) global force def display_gripper_state(msg): global force force = msg.force rospy.Subscriber('/robot/end_effector/left_gripper/state', EndEffectorState, display_gripper_state) with tf.Session() as sess: restore_vars(saver, sess, checkpoint_dir) attemp = 0 while True: # sample a grasp dx = np.random.rand() * (2. * delta) - delta dy = np.random.rand() * (2. * delta) - delta target_theta = (np.random.rand() * 2. - 1.) * 3.059 target_x = x0 + dx target_y = y0 + dy # move to the grasp location execute_linear(arm, target_x, target_y, initial_z, target_theta) # predict grasp crop = np.array( current_image.crop(crop_box).resize((width, width)))[:, :, :3] grasp_pred = grasp_class_prediction.eval(session=sess, feed_dict={ grasp_image_ph: crop.reshape( (1, width, width, 3)), keep_prob_ph: 1., }) # display image draw = PIL_ImageDraw.Draw(current_image) draw.text(crop_box[:2], 'prob: %.5f' % grasp_pred[0, 1]) draw.text((20, 20), 'grasp force: %.5f' % force) if grasp_pred[0, 1] > grasp_class_threashold: draw.rectangle(crop_box, outline=(0, 255, 0)) else: draw.rectangle(crop_box, outline=(0, 0, 255)) msg = Image( header=Header( stamp=rospy.Time.now(), frame_id='base', ), width=640, height=400, step=640 * 4, encoding='bgra8', is_bigendian=0, data=current_image.tobytes(), ) pub.publish(msg) if grasp_pred[0, 1] > grasp_class_threashold: execute_planar_grasp(arm, gripper, initial_z, bound_z, target_theta, lower_to_drop=0.05) attemp += 1
def main(): rospy.sleep(.2) global camdata #print "camdata is" ,camdata global right global lg global left #subscribe to all necessary sources tf_listener = tf2_ros.TransformListener(tf_buffer) #range finder rospy.Subscriber('/robot/range/left_hand_range/state', Range, callback_range, queue_size=1) #Alvar markers rospy.Subscriber('/ar_pose_marker', AlvarMarkers, callback_marker, queue_size=1) #endpoint state rospy.Subscriber('/robot/limb/left/endpoint_state', EndpointState, callback_endpoint, queue_size=1) #set speed right = baxter_interface.Limb('right') right.set_joint_position_speed(1.0) left = baxter_interface.Limb('left') left.set_joint_position_speed(1.0) camdata = Point() #left gripper lg = baxter_interface.Gripper('left') #calibrate gripper if lg.calibrated() == False: lg.calibrate() #rg.calibrate() lg.open() quit = "n" #starting points for arm pos = Point(x=0.50, y=0.182539067108, z=0.0998704377885) quat = Quaternion(x=0.0, y=1.0, z=0.0, w=0.0) pos_start = Point(x=0.578951563602, y=0.182539067108, z=0.3998704377885) # hard coded joint postion used for demos, not needed # high_joints = {'left_w0': 0.20747090156150222, 'left_w1': 1.3169225063996277, # 'left_w2': -0.37314082665312687, 'left_e0': -0.7098496095939753, # 'left_e1': 1.5523885573400387, 'left_s0': -0.4111068511532909, # 'left_s1': -1.2532623037023831} joints = ik_solve('left', pos_start, quat) left.move_to_joint_positions(joints) # Uncomment as apppropriate, find_surface takes some time so its best to # re-use this value for demos/debugging/repeated runs # Find surface_z and set with rangefinder surface_z = find_surface(pos_start, quat) # lab box height #surface_z = -0.314529563082 # lab small table height #surface_z = -0.0791295619731 #pre pose height pre_pose_high = Point(x=0.50, y=0.272539067108, z=0.3998704377885) print "pos at start" print pos_start.z print surface_z global poseglobal rd.seed(5) # Open grippers lg.open() # Main Grab loop for i in range(0, 100): print i # Allow time for grippers to drop a block if holding on rospy.sleep(0.5) # Move arm to a start position above the surface joints = ik_solve('left', pos, quat) if surface_z > -0.10: high_joints = ik_solve('left', pre_pose_high, quat) left.move_to_joint_positions(high_joints) else: left.move_to_joint_positions(joints) # Wait to for some position data to come in from a marker topic rospy.sleep(2) pos1 = update_arm_pos(pos) # construct a PoseStamped from pos1 pose_sm = Point(x=pos1.x + 0.05, y=pos1.y, z=surface_z + 0.15) ps = makePoseStamped(pose_sm, quat, "base") # wait for more data move to pre-grap pose for better accuracy rospy.sleep(1) pos1 = update_arm_pos(pos) pose_sm = Point(x=pos1.x + 0.05, y=pos1.y, z=surface_z + 0.16) joints = ik_solve('left', pose_sm, quat) left.move_to_joint_positions(joints) # wait for data in pre-grap pose, move to grab pose rospy.sleep(1) pose_grip = Point(x=pos1.x + 0.01, y=pos1.y, z=surface_z + 0.12) joints = ik_solve('left', pose_grip, quat) left.move_to_joint_positions(joints) # Grab block lg.close() rospy.sleep(0.5) # Move to randomised drop position magic numbers represent guesses for # sensible x and y bounds in base frame reference dropx = rd.uniform(0.40, 0.60) dropy = rd.uniform(0.18, 0.35) pose_drop = Point(x=dropx, y=dropy, z=surface_z + 0.15) joints = ik_solve('left', pose_drop, quat) left.move_to_joint_positions(joints) rospy.sleep(2) # Open grippers lg.open() print "clear" last_left_ar_posns.clear()
def map_keyboard(): done = False print("Controlling joints. Press ? for help, Esc to quit.") left = baxter_interface.Limb('left') right = baxter_interface.Limb('right') grip_left = baxter_interface.Gripper('left', CHECK_VERSION) grip_right = baxter_interface.Gripper('right', CHECK_VERSION) lj = left.joint_names() rj = right.joint_names() def set_j(limb, joint_name, delta): current_position = limb.joint_angle(joint_name) joint_command = {joint_name: current_position + delta} limb.set_joint_positions(joint_command) bindings = { # key: (function, args, description) '9': (set_j, [left, lj[0], 0.1], "left_s0 increase"), '6': (set_j, [left, lj[0], -0.1], "left_s0 decrease"), '8': (set_j, [left, lj[1], 0.1], "left_s1 increase"), '7': (set_j, [left, lj[1], -0.1], "left_s1 decrease"), 'o': (set_j, [left, lj[2], 0.1], "left_e0 increase"), 'y': (set_j, [left, lj[2], -0.1], "left_e0 decrease"), 'i': (set_j, [left, lj[3], 0.1], "left_e1 increase"), 'u': (set_j, [left, lj[3], -0.1], "left_e1 decrease"), 'l': (set_j, [left, lj[4], 0.1], "left_w0 increase"), 'h': (set_j, [left, lj[4], -0.1], "left_w0 decrease"), 'k': (set_j, [left, lj[5], 0.1], "left_w1 increase"), 'j': (set_j, [left, lj[5], -0.1], "left_w1 decrease"), '.': (set_j, [left, lj[6], 0.1], "left_w2 increase"), 'n': (set_j, [left, lj[6], -0.1], "left_w2 decrease"), ',': (grip_left.close, [], "left: gripper close"), 'm': (grip_left.open, [], "left: gripper open"), '/': (grip_left.calibrate, [], "left: gripper calibrate"), '4': (set_j, [right, rj[0], 0.1], "right_s0 increase"), '1': (set_j, [right, rj[0], -0.1], "right_s0 decrease"), '3': (set_j, [right, rj[1], 0.1], "right_s1 increase"), '2': (set_j, [right, rj[1], -0.1], "right_s1 decrease"), 'r': (set_j, [right, rj[2], 0.1], "right_e0 increase"), 'q': (set_j, [right, rj[2], -0.1], "right_e0 decrease"), 'e': (set_j, [right, rj[3], 0.1], "right_e1 increase"), 'w': (set_j, [right, rj[3], -0.1], "right_e1 decrease"), 'f': (set_j, [right, rj[4], 0.1], "right_w0 increase"), 'a': (set_j, [right, rj[4], -0.1], "right_w0 decrease"), 'd': (set_j, [right, rj[5], 0.1], "right_w1 increase"), 's': (set_j, [right, rj[5], -0.1], "right_w1 decrease"), 'v': (set_j, [right, rj[6], 0.1], "right_w2 increase"), 'z': (set_j, [right, rj[6], -0.1], "right_w2 decrease"), 'c': (grip_right.close, [], "right: gripper close"), 'x': (grip_right.open, [], "right: gripper open"), 'b': (grip_right.calibrate, [], "right: gripper calibrate"), } while not done and not rospy.is_shutdown(): c = getch() if c: #catch Esc or ctrl-c print "c:",c if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Example finished.") elif c in bindings: cmd = bindings[c] cmd[0](*cmd[1]) print("command: %s" % (cmd[2],)) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(bindings.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2]))
def main(): arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) required = parser.add_argument_group('required arguments') required.add_argument( '-l', '--limb', required=True, choices=['left', 'right'], help='send joint trajectory to which limb' ) args = parser.parse_args(rospy.myargv()[1:]) print args limb = args.limb print("Initializing node... ") rospy.init_node("super_stacker_%s" % (limb,)) print("Getting robot state... ") rs = baxter_interface.RobotEnable(CHECK_VERSION) print("Enabling robot... ") rs.enable() #Calibrate gripper gripper_if = baxter_interface.Gripper(limb) if not gripper_if.calibrated(): print "Calibrating gripper" gripper_if.calibrate() limbInterface = baxter_interface.Limb(limb) iksvc, ns = ik_command.connect_service(limb) rate = rospy.Rate(100) # Get goal poses of each object in the scene dc = DepthCaller(limb) # Subscribe to estimate_depth # Move to pose published by estimate_depth while (not dc.done) and (not rospy.is_shutdown()): rate.sleep() #pass # Subscribe to object_finder and start visual servoing/grasping # The stack pose is the pose of the smallest object with a z-offset # accounting for the height of the object (this assumption makes # it really important that the segmentation is accurate) stack_pose = [0.594676466827, -0.296644499519, -0.0322744943164, 0.971805911045, -0.22637170004, 0.065946440385, 0.000437813100735] #stack_pose = dc.object_poses[-1] #stack_pose = incrementPoseZ(stack_pose, object_height) #dc.object_poses.pop(len(dc.object_poses)-1) for pose in dc.object_poses: ik_command.service_request(iksvc, pose, limb, blocking=True) vc = VisualCommand(iksvc, limb) vc.subscribe() while (not vc.done) and (not rospy.is_shutdown()): rate.sleep() ik_command.service_request(iksvc, stack_pose, limb, blocking=True) #limbInterface.move_to_joint_positions(stack_pose) # Let go gripper_if.open(block=True) stack_pose = incrementPoseZ(pose, object_height)
cmd['left_e1'] = dp[3] cmd['left_w0'] = dp[4] cmd['left_w1'] = dp[5] cmd['left_w2'] = dp[6] # for i,joint in enumerate(leftarm.joint_names()): # cmd[joint]=dp[i] leftarm.set_joint_velocities(cmd) if __name__ == "__main__": rospy.init_node('baxter_kin') print 'initial node successd' rs = baxter_interface.RobotEnable(CHECK_VERSION) rs.enable() print 'enable baxter successed' leftarm = baxter_interface.Limb('left') print 'move leftarm to the initial position' leftarm.move_to_neutral() print '****** move leftarm in x direction ******' count = 0 while count < 500: Movejac() count = count + 1 print 'move leftarm to the initial position' leftarm.move_to_neutral() count = 0 print '****** move in Null space ******' while count < 100: nullspace()
def Step2(): limb1 = baxter_interface.Limb('left') limb = baxter_interface.Limb('right') limb.set_joint_position_speed(1.0) wave_1 = { 'right_s0': 0.7144515511413575, 'right_s1': -0.6699661083435059, 'right_w0': -1.740301201867676, 'right_w1': -1.570796325, 'right_w2': 0.018791264630126956, 'right_e0': 0.06250971703491211, 'right_e1': 0.704097180834961 } wave_2 = { 'left_w0': 3.03383049977417, 'left_w1': -1.5079031127685547, 'left_w2': -0.21207284368286133, 'left_e0': 0.15301458341674806, 'left_e1': 0.6707330987365723, 'left_s0': -0.9295923563964844, 'left_s1': -0.7221214550720215 } wave_3 = { 'right_s0': 0.7857816576965333, 'right_s1': -0.1955825502319336, 'right_w0': -2.869694555657959, 'right_w1': 1.609912835046387, 'right_w2': -3.0591411827453614, 'right_e0': -1.9665633678222658, 'right_e1': -0.04985437554931641 } wave_4 = { 'right_s0': 0.7857816576965333, 'right_s1': -0.1955825502319336, 'right_w0': -2.869694555657959, 'right_w1': 1.609912835046387, 'right_w2': 3.0591411827453614, 'right_e0': -1.9665633678222658, 'right_e1': -0.04985437554931641 } limb.move_to_joint_positions(wave_1) limb1.move_to_joint_positions(wave_2) t_end = time.time() + 5 limb.move_to_joint_positions(wave_3) limb.move_to_joint_positions(wave_4) #wave_8={'right_s0': 0.6396699878173828, 'right_s1': -0.49202433715209964, 'right_w0': -1.7138400333068848, 'right_w1': 1.255563273449707, 'right_w2': -0.6066894009155274, 'right_e0': -1.7429856682434084, 'right_e1': 1.0492428577148438} #limb.move_to_joint_positions(wave_8) wave_7 = { 'right_s0': 0.9639937033411371, 'right_s1': -0.28695082759045815, 'right_w0': -2.959, 'right_w1': 1.5803892485976359, 'right_w2': -2.9352389229115348, 'right_e0': -0.14089182454059407, 'right_e1': 1.8593947307842924 } limb.move_to_joint_positions(wave_7)
def __init__(self, ArmName): print("Getting robot state... ") self._rs = baxter_interface.RobotEnable() self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() self.torcmd_now = np.array([[0.0], [0.0], [0.0], [0.0], [0.0], [0.0], [0.0]]) self.name = ArmName self.limb = baxter_interface.Limb(ArmName) #self.limb.move_to_neutral() t1 = self.limb.joint_angles() t2 = [-0.528, -1.049, 1.131, 1.268, 0.618, 1.154, 2.416] temp = 0 for key in t1: t1[key] = t2[temp] temp = temp + 1 self.limb.move_to_joint_positions(t1) #self.limb.set_joint_position_speed(0.1) self.actual_effort = self.limb.joint_efforts() self.gravity_torques = self.limb.joint_efforts() self.final_torques = self.gravity_torques.copy() self.qnow = dict() # 得到关节角度的字典 self.qnow_value = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # 得到角度的值 self.torcmd = dict() # 给baxter赋值 self.torController0 = PIDController() self.torController1 = PIDController() self.torController2 = PIDController() self.torController3 = PIDController() self.torController4 = PIDController() self.torController5 = PIDController() self.torController6 = PIDController() self.torController = \ [self.torController0, self.torController1, self.torController2, self.torController3, self.torController4, self.torController5, self.torController6 ] '''设置PID参数''' # 最前端 self.torController[0].set_kp(17.7) # 130#80.0#*0.6 self.torController[0].set_ki(0.01) self.torController[0].set_kd(5.1) # 10#15#0.01#*0.6#21.0 self.torController[1].set_kp(15) # 130#80.0#*0.6 self.torController[1].set_ki(6) self.torController[1].set_kd(18) # 10#15#0.01#*0.6#21.0 self.torController[2].set_kp(15.7) # 130#80.0#*0.6 self.torController[2].set_ki(0.1) self.torController[2].set_kd(1.2) # 10#15#0.01#*0.6#21.0 self.torController[3].set_kp(10.02) # 130#80.0#*0.6 self.torController[3].set_ki(1.2) self.torController[3].set_kd(2.5) # 10#15#0.01#*0.6#21.0 self.torController[4].set_kp(10.3) self.torController[4].set_ki(0.1) #0.1 self.torController[4].set_kd(2.1) self.torController[5].set_kp(14.6) # 130#80.0#*0.6 self.torController[5].set_ki(1.5) #0.05 self.torController[5].set_kd(2.1) # 10#15#0.01#*0.6#21.0 self.torController[6].set_kp(22) # 130#80.0#*0.6 self.torController[6].set_ki(1.5) self.torController[6].set_kd(4.5) # 10#15#0.01#*0.6#21.0
def map_keyboard(right_gripper): left = baxter_interface.Limb('left') right = baxter_interface.Limb('right') grip_left = baxter_interface.Gripper('left', CHECK_VERSION) grip_right = baxter_interface.Gripper('right', CHECK_VERSION) lj = left.joint_names() rj = right.joint_names() def set_j(limb, joint_name, delta): current_position = limb.joint_angle(joint_name) joint_command = {joint_name: current_position + delta} limb.set_joint_positions(joint_command) bindings = { # key: (function, args, description) '9': (set_j, [left, lj[0], 0.1], "left_s0 increase"), '6': (set_j, [left, lj[0], -0.1], "left_s0 decrease"), '8': (set_j, [left, lj[1], 0.1], "left_s1 increase"), '7': (set_j, [left, lj[1], -0.1], "left_s1 decrease"), 'o': (set_j, [left, lj[2], 0.1], "left_e0 increase"), 'y': (set_j, [left, lj[2], -0.1], "left_e0 decrease"), 'i': (set_j, [left, lj[3], 0.1], "left_e1 increase"), 'u': (set_j, [left, lj[3], -0.1], "left_e1 decrease"), 'l': (set_j, [left, lj[4], 0.1], "left_w0 increase"), 'h': (set_j, [left, lj[4], -0.1], "left_w0 decrease"), 'k': (set_j, [left, lj[5], 0.1], "left_w1 increase"), 'j': (set_j, [left, lj[5], -0.1], "left_w1 decrease"), '.': (set_j, [left, lj[6], 0.1], "left_w2 increase"), 'n': (set_j, [left, lj[6], -0.1], "left_w2 decrease"), ',': (grip_left.close, [], "left: gripper close"), 'm': (grip_left.open, [], "left: gripper open"), '/': (grip_left.calibrate, [], "left: gripper calibrate"), '4': (set_j, [right, rj[0], 0.1], "right_s0 increase"), '1': (set_j, [right, rj[0], -0.1], "right_s0 decrease"), '3': (set_j, [right, rj[1], 0.1], "right_s1 increase"), '2': (set_j, [right, rj[1], -0.1], "right_s1 decrease"), 'r': (set_j, [right, rj[2], 0.1], "right_e0 increase"), 'q': (set_j, [right, rj[2], -0.1], "right_e0 decrease"), 'e': (set_j, [right, rj[3], 0.1], "right_e1 increase"), 'w': (set_j, [right, rj[3], -0.1], "right_e1 decrease"), 'f': (set_j, [right, rj[4], 0.1], "right_w0 increase"), 'a': (set_j, [right, rj[4], -0.1], "right_w0 decrease"), 'd': (set_j, [right, rj[5], 0.1], "right_w1 increase"), 's': (set_j, [right, rj[5], -0.1], "right_w1 decrease"), 'v': (set_j, [right, rj[6], 0.1], "right_w2 increase"), 'z': (set_j, [right, rj[6], -0.1], "right_w2 decrease"), 'c': (grip_right.close, [], "right: gripper close"), 'x': (grip_right.open, [], "right: gripper open"), 'b': (grip_right.calibrate, [], "right: gripper calibrate"), } done = False print("Controlling joints. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): # c = baxter_external_devices.getch() c = raw_input("Enter l' or 'r' and 7 joint angles: ") arm = right aj = rj if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Example finished.") if c == 'get': for i in range(7): print("joint " + str(i) + ": " + str(arm.joint_angle(aj[i]))) continue angles = c.split() angleList = [] if c[0] == 'l': arm = left aj = lj else: arm = right aj = rj print(arm) print(aj) #Close the left gripper print('Closing...') right_gripper.close(block=False) rospy.sleep(1.0) up_delta = arm.joint_angle(aj[1]) - 0.505446669 while abs(arm.joint_angle(aj[1]) - up_delta) > 0.01: joint_command = {aj[1]: up_delta} arm.set_joint_positions(joint_command) for i in range(1, len(angles)): angle_float = float(angles[i]) joint_command = {aj[i - 1]: angle_float} while abs(arm.joint_angle(aj[i - 1]) - angle_float) > 0.01: arm.set_joint_positions(joint_command) #Open the left gripper print('Opening...') right_gripper.open(block=True) rospy.sleep(1.0) print('Done!') for i in range(7): print("joint " + str(i) + ": " + str(arm.joint_angle(aj[i])))