def cartesianPath(xStart, xEnd, vStart, vEnd, aStart, aEnd, jStart, jEnd, T, N): Rs, ps = TransToRp(xStart) Re, pe = TransToRp(xEnd) tSpace = np.linspace(0, T, N) Rse = RotInv(Rs).dot(Re) i = 0 Xlist = np.empty((N, 4, 4)) Xlist[0] = Xstart a = np.array( [[1, 0, 0, 0, 0, 0, 0, 0], [1, T, T**2, T**3, T**4, T**5, T**6, T**7], [0, 1, 0, 0, 0, 0, 0, 0], [0, 1, 2 * T, 3 * T**2, 4 * T**3, 5 * T**4, 6 * T**5, 7 * T**6], [0, 0, 2, 0, 0, 0, 0, 0], [0, 0, 2, 6 * T, 12 * T**2, 20 * T**3, 30 * T**4, 42 * T**5], [0, 0, 0, 6, 0, 0, 0, 0], [0, 0, 0, 6, 24 * T, 60 * T**2, 120 * T**3, 210 * T**4]]) # b = np.array([q_start[i],q_throw[i],0,q_dot[i],0,0,0,jerk]) b = np.array([xStart[0, 3], xEnd[0, 3], 0, vEnd[3], 0, 0, 0, 0]) coeff = np.linalg.solve(a, b) for t in tSpace[1:]: i = i + 1 if timeScale == 3: s = CubicTimeScaling(T, t) elif timeScale == 5: s = QuinticTimeScaling(T, t) else: raise ValueError('timeScale must be 3 or 5') p = ps + s * (pe - ps) R = Rs.dot(MatrixExp3(MatrixLog3(Rse) * s)) Xlist[i] = RpToTrans(R, p) return Xlist
def move_to(pos): pub_demo_state = rospy.Publisher('demo_state', Int16, queue_size=10) if (pos == 1): catch = np.array([.65, .2, 0]) # my .7? R = np.array([[0.26397895, -0.34002068, 0.90260791], [-0.01747676, -0.93733484, -0.34799134], [0.96437009, 0.07608772, -0.25337913]]) elif (pos == 2): catch = np.array([.68, -.05, 0]) R = np.array([[0, 0, 1], [0, -1, 0], [1, 0, 0]]) elif (pos == 3): catch = np.array([.72, .1, 0]) R = np.array([[0.26397895, -0.34002068, 0.90260791], [-0.01747676, -0.93733484, -0.34799134], [0.96437009, 0.07608772, -0.25337913]]) else: pass th_init = np.array([-.3048, -.2703, -.1848, 1.908, .758, -1.234, -3.04]) X = RpToTrans(R, catch) # Find end joint angles with IK robot = URDF.from_parameter_server() base_link = robot.get_root() kdl_kin = KDLKinematics(robot, base_link, 'left_gripper_base') seed = 0 q_ik = kdl_kin.inverse(X, th_init) while q_ik == None: seed += 0.01 q_ik = kdl_kin.inverse(X, th_init + seed) if (seed > 1): # return False break q_goal = q_ik print q_goal limb_interface = baxter_interface.limb.Limb('left') angles = limb_interface.joint_angles() for ind, joint in enumerate(limb_interface.joint_names()): angles[joint] = q_goal[ind] limb_interface.move_to_joint_positions(angles) # rospy.sleep(5) print 'done' pub_demo_state.publish(1)
def sample_state(catch_x, catch_y, catch_z, pos): res = test_pos(catch_x, catch_y, catch_z, pos) while res == None: res = test_pos(catch_x, catch_y, catch_z, pos) throw_y = res[0] throw_z = res[1] vel = res[2] alpha = res[3] throw_x = .68 throw_pos = np.array([throw_x, throw_y, throw_z]) th_init = np.array([ 0.47668453, -0.77274282, 0.93150983, 2.08352941, 0.54149522, -1.26745163, -2.06742261 ]) # experimentally determined R_init = np.array([[0.06713617, -0.05831221, 0.99603836], [-0.97267055, -0.22621871, 0.05231732], [0.22227178, -0.97232956, -0.07190603]]) X = RpToTrans(R_init, throw_pos) return X, th_init, throw_y, throw_z, vel, alpha
def find_path(plot, pos): # Set initial position q_start = np.array([ -0.22281071, -0.36470393, 0.36163597, 1.71920897, -0.82719914, -1.16889336, -0.90888362 ]) limb_interface = baxter_interface.limb.Limb('right') angles = limb_interface.joint_angles() for ind, joint in enumerate(limb_interface.joint_names()): q_start[ind] = angles[joint] print q_start # Find goal for throwing if pos == 1: catch_y = .7 elif pos == 2: catch_y = .1 elif pos == 3: catch_y = .3 pos_goal = find_feasible_release(catch_x, catch_y, catch_z, pos) block_width = .047 throw_y = pos_goal[0] throw_z = pos_goal[1] dy = catch_y - throw_y - block_width vel = pos_goal[2] alpha = pos_goal[3] t = np.linspace(0, dy / (vel * cos(alpha)), 100) traj_y = vel * cos(alpha) * t + throw_y traj_z = -.5 * 9.8 * t**2 + vel * sin(alpha) * t + throw_z if (plot == True): plt.close('all') plt.figure() plt.hold(False) plt.plot(traj_y, traj_z, 'r', linewidth=2) plt.hold(True) plt.plot(traj_y[0], traj_z[0], 'r.', markersize=15) plt.ylim([-.8, .5]) plt.xlim([-.2, .8]) plt.xlabel('Y position (m)') plt.ylabel('Z position (m)') plt.title('Desired trajectory') plt.show(block=False) wm = plt.get_current_fig_manager() wm.window.wm_geometry("800x500+1000+0") # wm.window.setGeometry(800,500,0,0) raw_input("Press enter to continue...") # plt.show(block=False) print('found release state') print pos_goal # Add rotation to position and convert to rotation matrix R = np.array([[0.11121663, -0.14382586, 0.98333361], [-0.95290138, -0.2963578, 0.06442835], [0.28215212, -0.94418546, -0.17001177]]) p = np.hstack((0.68, pos_goal[0:2])) X = RpToTrans(R, p) # Find end joint angles with IK robot = URDF.from_parameter_server() base_link = robot.get_root() kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base') thInit = q_start seed = 0 q_ik = kdl_kin.inverse(X, thInit) while q_ik == None: seed += 0.01 q_ik = kdl_kin.inverse(X, thInit + seed) if (seed > 1): # return False break q_goal = q_ik # print q_goal # Transform to joint velocities using Jacobian jacobian = kdl_kin.jacobian(q_ik) inv_jac = np.linalg.pinv(jacobian) Vb = np.array([0, 0, 0, 0, pos_goal[2], pos_goal[3]]) q_dot_goal = inv_jac.dot(Vb) iter = 1000 treeA = np.empty((iter + 1, 14)) edgesA = np.empty((iter, 2)) treeB = np.empty((iter + 1, 14)) edgesB = np.empty((iter, 2)) treeA[0] = np.hstack((q_start, np.array([0, 0, 0, 0, 0, 0, 0]))) treeB[0] = np.hstack((q_goal, q_dot_goal.tolist()[0])) treeA, treeB, edgesA, edgesB, indA, indB = build_tree( iter, treeA, treeB, edgesA, edgesB, plot, kdl_kin) np.linalg.norm(q_start - q_goal) pathA = np.empty((edgesA.shape[0], 14)) pathA[0] = treeA[indA] curEdge = edgesA[indA - 1] ind = 1 atOrigin = False while atOrigin == False: pathA[ind] = treeA[curEdge[0]] curEdge = edgesA[curEdge[0] - 1] ind += 1 if curEdge[0] == 0: atOrigin = True pathA[ind] = treeA[curEdge[0]] pathA = pathA[0:ind + 1, :] pathB = np.empty((edgesB.shape[0], 14)) pathB[0] = treeB[indB] curEdge = edgesB[indB - 1] ind = 1 atOrigin = False # print treeB, pathB while atOrigin == False: pathB[ind] = treeB[curEdge[0]] curEdge = edgesB[curEdge[0] - 1] if curEdge[0] == 0: atOrigin = True # ind += 1 else: ind += 1 pathB[ind] = treeB[curEdge[0]] pathB = pathB[0:ind + 1, :] path = np.vstack((pathA[::-1], pathB)) if (plot): stepList = np.empty((path.shape[0], 3)) for i in range(path.shape[0]): stepList[i] = kdl_kin.forward(path[i, :7])[0:3, 3].transpose() plt.plot(stepList[:, 1], stepList[:, 2], 'g', linewidth=2) plt.show(block=False) raw_input('Press enter to continue...') plt.close('all') # if (plot): # plt.figure() # plt.plot(path[:,0:7],'.') # plt.show(block=False) # print(path.shape) return path
def main_func(): #Initialise Node and ROS variables rospy.init_node('tag_select', anonymous=False) rate = rospy.Rate(10) # 10hz listener = tf.TransformListener() #rtime = rospy.Time(0) #Initialize variables and empty lists trans = [] rot = [] dist = [] id_list = [] #tag_pose = Pose() tag_found = False global state ###Define Subsribers and Publishers #set callback for state rospy.Subscriber('state',Int16,set_state_callback) rospy.Subscriber("/cameras/right_hand_camera/image", Image,xdisplay_pub) state_pub = rospy.Publisher('state', Int16, queue_size=10, latch=True) tag_pub = rospy.Publisher('block_position', Pose, queue_size = 10, latch=True) goal_pub = rospy.Publisher('goal', Int16, queue_size = 10, latch=True) #Sleep Before entering loop rospy.sleep(.5) ''' try: (transWC,rotWC) = listener.lookupTransform('base', 'right_hand_camera', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue ''' ####Main ROS loop to run node until shutdown while not rospy.is_shutdown(): #IF statement to print current state if state ==1: print "Entered State :", state print "Is tag_found?:", tag_found ###Main State Machine Loop - Only run while state_callback is 1 while state == 1:# and tag_found == False: rospy.loginfo("Entered State 1") rtime = rospy.Time.now() trans = [] rot = [] dist = [] id_list = [] tag_found = False rospy.sleep(.15) print state ##For loop to search through tf frames of markers for n in range(1,7): marker_id = 'ar_marker_%d'%n #tag_found = listener.frameExists(marker_id) #print "Marker found is:", 'ar_marker_%d'%n #Check to see if the frames exists for the marker if listener.frameExists("/right_hand_camera") and listener.frameExists(marker_id): #set the time for the latest lookup rtime1 = listener.getLatestCommonTime("/right_hand_camera", marker_id) #check to see if this time is greater that time at beginning of loop if rtime1 > rtime: #set the tag_found to true tag_found = True rtime1 = rtime print "Marker list: AR_Marker", n, "exist:", tag_found ##when a frame is found for a particular Marker "n" add to list ##only 1 addition per "n" in for if tag_found == True: #print "looking at tag:",n #(transWC,rotWC) = listener.lookupTransform('base', 'right_hand_camera', rtime) posCB,quatCB = listener.lookupTransform('right_hand_camera', marker_id, rospy.Time(0)) #print "posCB:", posCB #print "quatCB", quatCB trans.append(posCB) rot.append(quatCB) dist.append(np.linalg.norm(posCB)) id_list.append(n) #print marker_id, ":", posCB tag_found = False ##IF Tag list is not empty we have a tag if len(dist)>0: #set tag_found tag_found = True #choose the closest tag found (if more than one) tag_index = np.argmin(dist) tag_selected = id_list[tag_index] tag_transform = (trans[tag_index],rot[tag_index]) print "tag index is", tag_index #get tag and world frames into correct variables posCB, quatCB = tag_transform posWC,quatWC = listener.lookupTransform('base', 'right_hand_camera', rospy.Time(0)) print "posCB from tf:", posCB #create proper format for pos and quat variables for functions posCB = np.array([posCB[0], posCB[1], posCB[2]]) posWC = np.array([posWC[0], posWC[1], posWC[2]]) quatCB = np.array([quatCB[3],quatCB[0],quatCB[1],quatCB[2]]) quatWC = np.array([quatWC[3],quatWC[0],quatWC[1],quatWC[2]]) #convert quat to Rotations rotWC = quat_to_so3(quatWC) rotCB = quat_to_so3(quatCB) #Build Transform SE3 matrix gWC = RpToTrans(rotWC,posWC) gCB = RpToTrans(rotCB,posCB) gWB = gWC.dot(gCB) #Reorient Orientation so that gripper is correct beta = pi gpick = gWB.dot(np.array([[cos(beta),0, -sin(beta),0],[0,1,0,0],[sin(beta),0, cos(beta),0],[0,0,0,1]])) #Recreate quatWB and rotWB (Block in world frame) from pickup Transform rotWB, posWB = TransToRp(gpick) quatWB = so3_to_quat(rotWB) #Create poses and orientations for block in world bpos = Pose() bpos.position.x = posWB[0] bpos.position.y = posWB[1] bpos.position.z = -.1489 #posWB[2] bpos.orientation.x = .99 bpos.orientation.y = -.024 bpos.orientation.z = .024 bpos.orientation.w = .0133 #print statements print "PosWB:", posWB print "Distance is:", dist[np.argmin(dist)] ##PUBLISH TAG if it Exists if tag_found == True: #Set locally and publish new state if tag_selected < 3: goal_pub.publish(1) else: goal_pub.publish(2) rospy.loginfo(2) state_pub.publish(2) #publish tage Pose tag_pub.publish(bpos) print "tag selected", tag_selected print state del posCB,quatCB,posWC,quatWC,posWB,quatWB ###Part of Main While rate.sleep()
def main_func(): #img = Image() rospy.init_node('tag_select', anonymous=False) rate = rospy.Rate(10) # 10hz count = 0 first_count = True listener = tf.TransformListener() trans = [] rot = [] dist = [] id_list = [] tag_pose = Pose() tag_found = False state = 1 rtime = rospy.Time(0) rospy.Subscriber('state',Int16,set_state_callback) # while listener.frameExists('base') == False: # try: # (transWC,rotWC) = listener.lookupTransform('base', 'right_hand_camera', rospy.Time(0)) # except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): # continue rospy.sleep(1) while not rospy.is_shutdown(): #if count%10==0: rospy.Subscriber("/cameras/right_hand_camera/image", Image,xdisplay_pub,count) #tag_found, tag_transform, WCtransform = multi_marker(listener,tag_found) while tag_found == False: #posCB, quatCB, posWC, quatWC = multi_marker(listener) for n in range(1,7): marker_id = 'ar_marker_%d'%n marker_found = listener.frameExists(marker_id) if marker_found == True: #print "looking at tag:",n #(transWC,rotWC) = listener.lookupTransform('base', 'right_hand_camera', rtime) posCB,quatCB = listener.lookupTransform('right_hand_camera', marker_id, rtime) print "posCB:", posCB print "quatCB", quatCB trans.append(posCB) rot.append(quatCB) dist.append(np.linalg.norm(posCB)) id_list.append(n) print marker_id, ":", posCB if len(dist)>0: tag_index = np.argmin(dist) print "tag index is", tag_index tag_selected = id_list[tag_index] tag_transform = (trans[tag_index],rot[tag_index]) tag_found = True #listener.waitForTransform('base', 'right_hand_camera', rospy.Time(0), rospy.Duration(.5)) posCB, quatCB = tag_transform posWC,quatWC = listener.lookupTransform('base', 'right_hand_camera', rospy.Time(0)) print "posCB after if:", posCB print "quatCB acter if:", quatCB print "posWC", posWC print "quatWC", quatWC posCB = np.array([posCB[0], posCB[1], posCB[2]]) posWC = np.array([posWC[0], posWC[1], posWC[2]]) quatCB = np.array([quatCB[3],quatCB[0],quatCB[1],quatCB[2]]) quatWC = np.array([quatWC[3],quatWC[0],quatWC[1],quatWC[2]]) # print tag_transform print "Distance is:", dist[np.argmin(dist)] count += 1 #if tag_found == True: #pub_tag_pose(tag_transform, WCtransform) #print tag_transform rotWC = quat_to_so3(quatWC) rotCB = quat_to_so3(quatCB) gWC = RpToTrans(rotWC,posWC) print "gWC is: ", gWC gCB = RpToTrans(rotCB,posCB) gWB = gWC.dot(gCB) beta = pi gpick = gWB.dot(np.array([[cos(beta),0, -sin(beta),0],[0,1,0,0],[sin(beta),0, cos(beta),0],[0,0,0,1]])) print "gWB is: ", gWB print "gCB is: ", gCB rotWB, posWB = TransToRp(gpick) quatWB = so3_to_quat(rotWB) print "quatWB", quatWB print "PosWB:", posWB bpos = Pose() bpos.position.x = posWB[0] bpos.position.y = posWB[1] bpos.position.z = posWB[2] bpos.orientation.x = quatWB[1] bpos.orientation.y = quatWB[2] bpos.orientation.z = quatWB[3] bpos.orientation.w = quatWB[0] tag_found = False state_pub = rospy.Publisher('state', Int16, queue_size=10) tag_pub = rospy.Publisher('block_position', Pose, queue_size = 10) for i in range(10): # state_pub = rospy.Publisher('state', Int16, queue_size=10) state_pub.publish(2) # tag_pub = rospy.Publisher('block_position', Pose, queue_size = 10) tag_pub.publish(bpos) print "tag selected", tag_selected #rospy.sleep(1) #break print state rate.sleep()
def main_func(): #Initialise Node and ROS variables rospy.init_node('tag_select', anonymous=False) rate = rospy.Rate(10) # 10hz listener = tf.TransformListener() #rtime = rospy.Time(0) #Initialize variables and empty lists trans = [] rot = [] dist = [] id_list = [] #tag_pose = Pose() tag_found = False global state ###Define Subsribers and Publishers #set callback for state rospy.Subscriber('state', Int16, set_state_callback) rospy.Subscriber("/cameras/right_hand_camera/image", Image, xdisplay_pub) state_pub = rospy.Publisher('state', Int16, queue_size=10, latch=True) tag_pub = rospy.Publisher('block_position', Pose, queue_size=10, latch=True) goal_pub = rospy.Publisher('goal', Int16, queue_size=10, latch=True) #Sleep Before entering loop rospy.sleep(.5) ''' try: (transWC,rotWC) = listener.lookupTransform('base', 'right_hand_camera', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue ''' ####Main ROS loop to run node until shutdown while not rospy.is_shutdown(): #IF statement to print current state if state == 1: print "Entered State :", state print "Is tag_found?:", tag_found ###Main State Machine Loop - Only run while state_callback is 1 while state == 1: # and tag_found == False: rospy.loginfo("Entered State 1") rtime = rospy.Time.now() trans = [] rot = [] dist = [] id_list = [] tag_found = False rospy.sleep(.15) print state ##For loop to search through tf frames of markers for n in range(1, 7): marker_id = 'ar_marker_%d' % n #tag_found = listener.frameExists(marker_id) #print "Marker found is:", 'ar_marker_%d'%n #Check to see if the frames exists for the marker if listener.frameExists("/right_hand_camera" ) and listener.frameExists(marker_id): #set the time for the latest lookup rtime1 = listener.getLatestCommonTime( "/right_hand_camera", marker_id) #check to see if this time is greater that time at beginning of loop if rtime1 > rtime: #set the tag_found to true tag_found = True rtime1 = rtime print "Marker list: AR_Marker", n, "exist:", tag_found ##when a frame is found for a particular Marker "n" add to list ##only 1 addition per "n" in for if tag_found == True: #print "looking at tag:",n #(transWC,rotWC) = listener.lookupTransform('base', 'right_hand_camera', rtime) posCB, quatCB = listener.lookupTransform( 'right_hand_camera', marker_id, rospy.Time(0)) #print "posCB:", posCB #print "quatCB", quatCB trans.append(posCB) rot.append(quatCB) dist.append(np.linalg.norm(posCB)) id_list.append(n) #print marker_id, ":", posCB tag_found = False ##IF Tag list is not empty we have a tag if len(dist) > 0: #set tag_found tag_found = True #choose the closest tag found (if more than one) tag_index = np.argmin(dist) tag_selected = id_list[tag_index] tag_transform = (trans[tag_index], rot[tag_index]) print "tag index is", tag_index #get tag and world frames into correct variables posCB, quatCB = tag_transform posWC, quatWC = listener.lookupTransform( 'base', 'right_hand_camera', rospy.Time(0)) print "posCB from tf:", posCB #create proper format for pos and quat variables for functions posCB = np.array([posCB[0], posCB[1], posCB[2]]) posWC = np.array([posWC[0], posWC[1], posWC[2]]) quatCB = np.array([quatCB[3], quatCB[0], quatCB[1], quatCB[2]]) quatWC = np.array([quatWC[3], quatWC[0], quatWC[1], quatWC[2]]) #convert quat to Rotations rotWC = quat_to_so3(quatWC) rotCB = quat_to_so3(quatCB) #Build Transform SE3 matrix gWC = RpToTrans(rotWC, posWC) gCB = RpToTrans(rotCB, posCB) gWB = gWC.dot(gCB) #Reorient Orientation so that gripper is correct beta = pi gpick = gWB.dot( np.array([[cos(beta), 0, -sin(beta), 0], [0, 1, 0, 0], [sin(beta), 0, cos(beta), 0], [0, 0, 0, 1]])) #Recreate quatWB and rotWB (Block in world frame) from pickup Transform rotWB, posWB = TransToRp(gpick) quatWB = so3_to_quat(rotWB) #Create poses and orientations for block in world bpos = Pose() bpos.position.x = posWB[0] bpos.position.y = posWB[1] bpos.position.z = -.1489 #posWB[2] bpos.orientation.x = .99 bpos.orientation.y = -.024 bpos.orientation.z = .024 bpos.orientation.w = .0133 #print statements print "PosWB:", posWB print "Distance is:", dist[np.argmin(dist)] ##PUBLISH TAG if it Exists if tag_found == True: #Set locally and publish new state if tag_selected < 3: goal_pub.publish(1) else: goal_pub.publish(2) rospy.loginfo(2) state_pub.publish(2) #publish tage Pose tag_pub.publish(bpos) print "tag selected", tag_selected print state del posCB, quatCB, posWC, quatWC, posWB, quatWB ###Part of Main While rate.sleep()
def linearSpace(plot, T, N, vy, vz, jerk): robot = URDF.from_parameter_server() base_link = robot.get_root() kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base') # Create seed with current position q0 = kdl_kin.random_joint_angles() limb_interface = baxter_interface.limb.Limb('right') current_angles = [ limb_interface.joint_angle(joint) for joint in limb_interface.joint_names() ] for ind in range(len(q0)): q0[ind] = current_angles[ind] # q_start = np.array([0.2339320701525256, -0.5878981369570848, 0.19903400722813244, 1.8561167533413507, # -0.4908738521233324, -0.97752925707998, -0.49547579448698864]) # q_throw = np.array([0.9265243958827899, -0.7827136970185323, -0.095490304045867, 1.8338740319170121, # -0.03681553890924993, -0.9909515889739773, -0.5840631849873713]) # q_end = np.array([0.9085001216251363, -1.0089758632316308, 0.07401457301547121, 1.8768254939778037, # 0.18599517053110642, -0.8172282647459542, -0.44600491407768406]) q_throw = np.array([ 0.47668453, -0.77274282, 0.93150983, 2.08352941, 0.54149522, -1.26745163, -2.06742261 ]) q_end = np.array([ 0.75356806, -0.89162633, 0.54648066, 2.08698086, 0.41033986, -1.18423317, -1.15815549 ]) q_start = np.array([ -0.22281071, -0.36470393, 0.36163597, 1.71920897, -0.82719914, -1.16889336, -0.90888362 ]) X_start = np.asarray(kdl_kin.forward(q_start)) X_throw = np.asarray(kdl_kin.forward(q_throw)) # offset throw position X_throw[0, 3] += 0 X_throw[1, 3] += 0 X_throw[2, 3] += 0 X_end = np.asarray(kdl_kin.forward(q_end)) Vb = np.array([0, 0, 0, 0, vy, vz]) # xStart = X_start # xEnd = X_throw vStart = np.array([0, 0, 0, 0, 0, 0]) vEnd = Vb aStart = np.array([0, 0, 0, 0, 0, 0]) aEnd = np.array([0, 0, 0, 0, 0, 0]) jStart = np.array([0, 0, 0, 0, 0, 0]) jEnd = np.array([0, 0, 0, 0, 0, jerk]) # T = .7 # N = 200*T a = np.array( [[1, 0, 0, 0, 0, 0, 0, 0], [1, T, T**2, T**3, T**4, T**5, T**6, T**7], [0, 1, 0, 0, 0, 0, 0, 0], [0, 1, 2 * T, 3 * T**2, 4 * T**3, 5 * T**4, 6 * T**5, 7 * T**6], [0, 0, 2, 0, 0, 0, 0, 0], [0, 0, 2, 6 * T, 12 * T**2, 20 * T**3, 30 * T**4, 42 * T**5], [0, 0, 0, 6, 0, 0, 0, 0], [0, 0, 0, 6, 24 * T, 60 * T**2, 120 * T**3, 210 * T**4]]) tSpace = np.linspace(0, T, N) tOffset = T colors = ['r', 'b', 'c', 'y', 'm', 'oc', 'k'] if plot == True: plt.close('all') pLista = np.empty((3, N)) pListb = np.empty((3, N)) vLista = np.empty((3, N)) vListb = np.empty((3, N)) aLista = np.empty((3, N)) aListb = np.empty((3, N)) for i in range(3): b = np.array([ X_start[i, 3], X_throw[i, 3], 0, vEnd[3 + i], 0, 0, jStart[i + 3], jEnd[i + 3] ]) coeff = np.linalg.solve(a, b) j1Pa = jointPath(tSpace, coeff) j1Va = jointVelocity(tSpace, coeff) j1Aa = jointAcceleration(tSpace, coeff) b = np.array([ X_throw[i, 3], X_end[i, 3], 0, 0, 0, 0, jEnd[i + 3], jStart[i + 3] ]) # b = np.array([X_throw[i,3],X_end[i,3],vEnd[3+i],0,0,0,jEnd[i+3],jStart[i+3]]) coeff = np.linalg.solve(a, b) j1Pb = jointPath(tSpace, coeff) j1Vb = jointVelocity(tSpace, coeff) j1Ab = jointAcceleration(tSpace, coeff) pLista[i, :] = j1Pa pListb[i, :] = j1Pb vLista[i, :] = j1Va vListb[i, :] = j1Vb aLista[i, :] = j1Aa aListb[i, :] = j1Ab vList = np.hstack((vLista, vListb)).transpose() dt = float(T) / (N - 1) if plot == True: plt.figure() plt.title('Position (cartesian)') plt.plot(np.hstack((pLista, pListb)).transpose()) plt.figure() plt.title('Velocity (cartesian)') plt.plot(np.hstack((vLista, vListb)).transpose()) # plt.figure() # plt.title('Velocity dt') # vList = np.diff(pLista)/dt # plt.plot(vList.transpose()) plt.figure() plt.title('Acceleration (cartesian)') plt.plot(np.hstack((aLista, aListb)).transpose()) plt.show(block=False) Xlista = np.empty((N, 4, 4)) Xlistb = np.empty((N, 4, 4)) Re, pe = TransToRp(X_throw) i = 0 for t in tSpace[0:]: pa = pLista[:, i] Ra = Re Xlista[i] = RpToTrans(Ra, pa) pb = pListb[:, i] Rb = Re Xlistb[i] = RpToTrans(Rb, pb) i = i + 1 XlistT = np.vstack((Xlista, Xlistb[1:, :, :])) q0 = kdl_kin.random_joint_angles() current_angles = [ limb_interface.joint_angle(joint) for joint in limb_interface.joint_names() ] for ind in range(len(q0)): q0[ind] = q_start[ind] thList = np.empty((N * 2 - 1, 7)) thList[0] = q0 for i in range(int(2 * N) - 2): # Solve for joint angles seed = 0 q_ik = kdl_kin.inverse(XlistT[i + 1], thList[i]) while q_ik == None: seed += 0.03 q_ik = kdl_kin.inverse(XlistT[i + 1], thList[i] + seed) if (seed > 1): q_ik = thList[i] break thList[i + 1] = q_ik thList = thList[1:, :] # if plot == True: # plt.figure() # plt.plot(thList) # plt.show(block=False) jList = np.empty((thList.shape[0], 7)) # Compare jacobian velocities to joint velocities for i in range(thList.shape[0]): jacobian = kdl_kin.jacobian(thList[i]) inv_jac = np.linalg.pinv(jacobian) Vb = np.hstack((np.array([0, 0, 0]), vList[i])) q_dot_throw = inv_jac.dot(Vb) jList[i, :] = q_dot_throw # plt.figure() # plt.title('Jacobian-based joint velocities') # plt.plot(jList); # plt.show(block = False) vList = np.diff(thList, axis=0) / dt vList = np.vstack((np.array([0, 0, 0, 0, 0, 0, 0]), vList)) vCarlist = np.empty((thList.shape[0], 6)) for i in range(thList.shape[0]): jacobian = kdl_kin.jacobian(thList[i]) vCar = jacobian.dot(vList[i]) vCarlist[i, :] = vCar if (plot == True): plt.figure() plt.title('Jacobian based cartesian velocities') plt.plot(vCarlist[:, 0:3]) plt.show(block=False) if (plot == True): plt.figure() plt.title('Joint velocities') plt.plot(vList) plt.show(block=False) return (thList, jList)