def inverse_dynamics(thetaList, d_thetaList, dd_thetaList, g, F_tip, M_list, G_list, S_list):
n = len(thetaList)
# initializing the twist, and twist dot
Vi = np.zeros((6,n+1)) # if there are 3 joints, then there are 4 twists to find (n+1)
Vdi = np.zeros((6,n+1))
Vdi[:,0] = np.r_[np.array([0,0,0]), -np.array(g)]
# first acceleration is from gravity, dont know where the negative came from
# this notation [:,0] is the first column
# np.array is for case where g is inputed as python array, np.array works fine for g inputed as numpy array
Mi = np.identity(4) # for the transfer from space axis to body axis, need the transf from {0} to the joint axis {i}
Ai = np.zeros((6,n)) # body axes, need to store for second for loop
AdTi = [[None]] * (n+1) # Adjoint of the transf_i_i-1, storing for second for loop
AdTi[n] = ch3.adjoint_transf_matrix(ch3.transf_matrix_inverse(M_list[n]))
# second for loop wants the transf from last frame to tip, and first for loop doesnt provide it
Fi = np.array(F_tip).copy() # copy by value, no link
tau_list = np.zeros(n) # initialize torque return list
for i in range(n): # 0 --> n-1
Mi = np.dot(Mi, Mlist[i]) #previous iterations dotted with current link config (this ends up being current link referenced to {0})
Ai[:,i] = np.dot(ch3.adjoint_transf_matrix(ch3.transf_matrix_inverse(Mi)), np.array(S_list)[:,i]) # Ai = Ad_(T_(i,i-1)) * Si
# use adjoint_transf to convert a space frame screw axis Si, into screw axis of joint i in {i} Ai
# Mi is T_sb, base relative to space; but to convert space axis to body axis--> Ab = T_bs*S_s (need to invert Mi)
# Ti = e^(-A_i*theta)*M_i,i-1
Ti = np.dot(ch3.se3_to_transf_matrix(ch3.vector6_to_se3(Ai[:, i]* -1*thetaList[i])),ch3.transf_matrix_inverse(Mlist[i]))
# Ti is Mi,i-1 with the joints at the respective angles (M is home config, T is final config)
# given Mi,i+1 in Mlist, need to invert it to fit in this equation
# the [:,x] notation is editing the column, index of x
AdTi[i] = ch3.adjoint_transf_matrix(Ti)
# Vi = Ad_Ti,i-1(Vi-1) + A_i*theta_dot
Va = np.dot(AdTi[i],Vi[:,i]) # twist of the previous link {i-1}, expressed in the current link {i} by the adjoint
Vb = Ai[:,i]*d_thetaList[i] # joint rate theta_dot about the screw axis Ai
Vi[:,i+1] = Va + Vb
# the [:,x] notation is editing the column, index of x
# dVi = Ad_Ti,i-1(dVi-1) + ad_Vi(Ai)*theta_dot + A_i*theta_ddot
Vda = np.dot(AdTi[i], Vdi[:,i]) # accleration of previous link {i-1}, expressed in current link {i} by adjoint
Vdb = np.dot(adjoint_twist(Vi[:,i+1]),Ai[:,i]) * d_thetaList[i] # velocity product component (came from derivative of AdTi from Vi)
Vdc = Ai[:,i]*dd_thetaList[i] # joint acceleration dd_theta around the screw axis Ai
Vdi[:,i+1] = Vda+Vdb+Vdc
for i in range(n-1,-1,-1):
Fa = np.dot(np.array(AdTi[i+1]).T, Fi) # wrench applied to link i thru joint i+1
Fb = np.dot(G_list[i],Vdi[:,i+1]) # wrench applied to link i thru joint i, G_b*dV_b
Fc = np.dot(np.array(adjoint_twist(Vi[:,i+1])).T, np.dot(G_list[i], Vi[:,i+1])) # wrench applied to link i thru joint i (ad_Vb)^T*G_b*V_b
Fi = Fa+Fb-Fc
tau_list[i] = np.dot(np.array(Fi).T, Ai[:, i])
return tau_list
def inverse_dynamics_bad(thetaList, d_thetaList, dd_thetaList, g, fTip, MList, GList, SList):
forces = [np.array(fTip)]
torques = []
transfs = []
twists = [np.array([0,0,0,0,0,0])] # first twist shouldn't be moving if arm is stationary, if attached to a wheeled platform then yeah change it
twistDots = [np.append(np.zeros(3),-1*g)]
# have to edit both for loops for i=1..n, i=n...1 (cut some lists up) hard
for (theta, dtheta, ddtheta, M, S) in zip(thetaList, d_thetaList, dd_thetaList, MList[1:], np.transpose(SList)):
#first need to find the transf matrix T_(i+1,i) using M,A,thetaand matrix exponentials
A = np.dot(ch3.adjoint_transf_matrix(ch3.transf_matrix_inverse(Mi)), np.array(S_list)[:,i])
T = ch3.screwtheta_to_transf_matrix(A,theta) @ M
V = np.dot(ch3.adjoint_transf_matrix(T),twists[-1]) + A*dtheta
Vdot = np.dot(ch3.adjoint_transf_matrix(T), twistDots[-1]) + np.dot(adjoint_twist(V),A)*dtheta + A*ddtheta
transfs.append(T)
twists.append(V)
twistDots.append(Vdot)
# twists and twistDots contain the zeroth twist and twistDot, but this zip will stop at the smallest list end, so it fine
for (twist, twistDot, G, A) in zip(twists[::-1], twistDots[::-1], GList[::-1], np.transpose(SList)[::-1]):
#first need to find the transf matrix T_(i+1,i) using M,A,thetaand matrix exponentials
first = np.dot(ch3.adjoint_transf_matrix(T),forces[0])
second = np.dot(G,twistDot)
third = np.dot(adjoint_twist(V),(np.dot(G,twist)))
force = first+second-third
tau = np.dot(force,A)
# np.column_stack((forces,force))
forces.insert(0,force)
torques.insert(0,tau)
return torques
def screw_trajectory(X_start, X_end, Tf, N, method):
"""Computes a trajectory as a list of N SE(3) matrices corresponding to
the screw motion about a space screw axis (each matrix represents the config of the end effector at an instant in time)
:param Xstart: The initial end-effector configuration
:param Xend: The final end-effector configuration
:param Tf: Total time of the motion in seconds from rest to rest
:param N: The number of points N > 1 (Start and stop) in the discrete
representation of the trajectory
:param method: The time-scaling method, where 3 indicates cubic (third-
order polynomial) time scaling and 5 indicates quintic
(fifth-order polynomial) time scaling
:return: The discretized trajectory as a list of N matrices in SE(3)
separated in time by Tf/(N-1). The first in the list is Xstart
and the Nth is Xend"""
N = int(N)
timegap = Tf / (N - 1.0)
traj = [[None]] * N
X_start_end = np.dot(ch3.transf_matrix_inverse(X_start), X_end)
for i in range(N):
if method == 3:
s = cubic_time_scaling(Tf, timegap * i)
else:
s = quintic_time_scaling(Tf, timegap * i)
fractioned_X_start_end = ch3.se3_to_transf_matrix(
ch3.transf_matrix_to_se3(X_start_end) * s
) # applying s to the se3 matrix instead of the transf matrix, it works
traj[i] = np.dot(X_start, fractioned_X_start_end)
return traj
def IK_body(Blist, M, T, thetalist0, e_omega, e_v):
"""uses iterative Netwon Raphson to calculate inverse kinematics
Args:
Blist : list of joint screws expressed in the end effector frame scriptB
M : end effector home configuration
T : desired end effector configuration (T_sd)
thetalist0: initial guess
e_omega : omega error tolerance for stoppage
e_v : linear velocity error tolerance for stoppage
"""
thetaList = thetalist0
# finds transf matrix of current guess T_sb
T_sb = ch4.forward_kinematics_in_body(M, Blist, thetaList)
T_bs = ch3.transf_matrix_inverse(T_sb)
# finds the se3 for the transf matrix T_bd (transformation from current guess to answer)
(scriptV_b_bracketed, theta) = ch3.transf_matrix_to_se3(T_bs @ T)
scriptV_b_exp6 = ch3.se3_to_vector6(scriptV_b_bracketed*theta)
# these velocities are required to get from T_sb (current) to T_sd (target)
# - want to bring these to zero
omega = scriptV_b_exp6[0:3]
v = scriptV_b_exp6[3:6]
counter = 0
print(ch3.magnitude(omega))
print(ch3.magnitude(v))
while((ch3.magnitude(omega) > e_omega or ch3.magnitude(v) > e_v) and counter < 25):
# use the jacobian body in chapter 5 to see what velocities can be generated by each angle velocity
#- uses this stupid pseudo inverse thing that i missed
jacobian_b = np.linalg.pinv(ch5.jacobian_body(Blist, thetaList))
# multiplying the jacobian by the twist gives an angle to add to
thetaListaddition = np.dot(jacobian_b, np.reshape(scriptV_b_exp6, (-1,1)))
# add said angle
thetaList = [sum(i) for i in zip(thetaList,thetaListaddition.flatten().tolist())]
# recalculate current transf matrix, scriptV, screw, omega, and v
T_sb = ch4.forward_kinematics_in_body(M, Blist, thetaList)
T_bs = ch3.transf_matrix_inverse(T_sb)
(scriptV_b_bracketed, theta) = ch3.transf_matrix_to_se3(T_bs @ T)
scriptV_b_exp6 = ch3.se3_to_vector6(scriptV_b_bracketed*theta)
omega = scriptV_b_exp6[0:3]
v = scriptV_b_exp6[3:6]
counter += 1
return (thetaList, counter < 25)
def testingJacobianWithExampleFromGithub():
R = np.identity(3)
p = np.array([0, 2, 1])
M = ch3.Rp_to_transf_matrix(R, p)
thetaList = [0.2, 1.1, 0.1, 1.2]
screwSpaceAxis1 = np.array([0, 0, 1, 0, 0.2, 0.2])
screwSpaceAxis2 = np.array([1, 0, 0, 2, 0, 3])
screwSpaceAxis3 = np.array([0, 1, 0, 0, 2, 1])
screwSpaceAxis4 = np.array([1, 0, 0, 0.2, 0.3, 0.4])
screwSpaceList = [
screwSpaceAxis1, screwSpaceAxis2, screwSpaceAxis3, screwSpaceAxis4
]
screwBodyList = []
for screwSpaceAxis in screwSpaceList:
screwBodyList.append(
np.dot(ch3.adjoint(ch3.transf_matrix_inverse(M)), screwSpaceAxis))
FK_space = ch5.jacobian_space(screwSpaceList, thetaList)
print(FK_space)
FK_body = ch5.jacobian_body(screwSpaceList, thetaList)
print(FK_body)
def testingJacobian():
R = np.identity(3)
p = np.array([0, 2, 1])
M = ch3.Rp_to_transf_matrix(R, p)
thetaList = [np.pi / 2, np.pi / 2, -1 * np.pi / 2, 1]
screwSpaceAxis1 = ch3.qsh_to_screwaxis(np.array([0, 0, 0]),
np.array([0, 0, 1]), 0)
screwSpaceAxis2 = ch3.qsh_to_screwaxis(np.array([0, 1, 0]),
np.array([0, 0, 1]), 0)
screwSpaceAxis3 = ch3.qsh_to_screwaxis(np.array([0, 2, 0]),
np.array([0, 0, 1]), 0)
screwSpaceAxis4 = np.array([0, 0, 0, 0, 0, 1])
screwSpaceList = [
screwSpaceAxis1, screwSpaceAxis2, screwSpaceAxis3, screwSpaceAxis4
]
screwBodyList = []
for screwSpaceAxis in screwSpaceList:
screwBodyList.append(
np.dot(ch3.adjoint(ch3.transf_matrix_inverse(M)), screwSpaceAxis))
FK_space = ch5.jacobian_space(screwSpaceList, thetaList)
print(FK_space)
def testingJacobianWithExample():
R = np.identity(3)
p = np.array([0, 2, 1])
M = ch3.Rp_to_transf_matrix(R, p)
thetaList = [0, 0, 1 * np.pi / 2, 1]
screwSpaceAxis1 = np.array([0, 0, 1, 0, 0, 0])
screwSpaceAxis2 = np.array(
[0, 0, 1, 2 * math.sin(thetaList[0]), -2 * math.cos(thetaList[0]), 0])
screwSpaceAxis3 = np.array([
0, 0, 1, 2 * math.sin(thetaList[0]) + 3 * math.sin(thetaList[1]),
-2 * math.cos(thetaList[0]) - 3 * math.cos(thetaList[1]), 0
])
screwSpaceAxis4 = np.array([0, 0, 0, 0, 0, 1])
screwSpaceList = [
screwSpaceAxis1, screwSpaceAxis2, screwSpaceAxis3, screwSpaceAxis4
]
screwBodyList = []
for screwSpaceAxis in screwSpaceList:
screwBodyList.append(
np.dot(ch3.adjoint(ch3.transf_matrix_inverse(M)), screwSpaceAxis))
FK_space = ch5.jacobian_space(screwSpaceList, thetaList)
print(FK_space)