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)
