def FeedbackControl(X, Xd, Xd_next, Kp, Ki, delta_t, current_config,
error_integral):
""" This function calculates the kinematic task-space feedforward plus feedback control law
(written both as Equation (11.16) and (13.37) in the textbook).
Input:
X - The current actual end-effector configuration (Tse).
Xd - The current end-effector reference configuration (Tse_d).
Xd_next - The end-effector reference configuration at the next timestep in the reference trajectory (Tse_d_next).
Kp - the P gain matrix.
Ki - the I gain matrix.
delta_t - The time step Δt between reference trajectory configurations.
current_config - The current cunfiguration.
Return:
V - The commanded end-effector twist, expressed in the end-effector frame {e}.
controls - The commanded wheel and arm joint controls.
Xerr - The error.
error_integral - The error integral.
"""
# Initialize variables:
l = 0.47 / 2 # The forward-backward distance between the wheels to frame {b} [m]
w = 0.3 / 2 # The side-to-side distance between the wheels to frame {b} [m]
r = 0.0475 # The radius of each wheel [m]
# The fixed offset from the chassis frame {b} to the base frame of the arm {0}:
Tb0 = np.array([[1, 0, 0, 0.1662], [0, 1, 0, 0], [0, 0, 1, 0.0026],
[0, 0, 0, 1]])
# The end-effector frame {e} relative to the arm base frame {0}, when the arm is at its home configuration:
M0e = np.array([[1, 0, 0, 0.033], [0, 1, 0, 0], [0, 0, 1, 0.6546],
[0, 0, 0, 1]])
# The screw axes B for the 5 joints expressed in the end-effector frame {e}, when the arm is at its home configuration:
Blist = np.array([[0, 0, 1, 0, 0.0330, 0], [0, -1, 0, -0.5076, 0, 0],
[0, -1, 0, -0.3526, 0, 0], [0, -1, 0, -0.2176, 0, 0],
[0, 0, 1, 0, 0, 0]]).T
# Get current arm joint angles:
arm_joints = current_config[3:8]
# Calculate the chasis configuration (according to Chapter 13.4):
F = r / 4 * np.array(
[[0, 0, 0, 0], [0, 0, 0, 0],
[-1 / (l + w), 1 / (l + w), 1 / (l + w), -1 /
(l + w)], [1, 1, 1, 1], [-1, 1, -1, 1], [0, 0, 0, 0]])
# The fixed offset from the base frame of the arm {0} to the end-effector frame {e}:
T0e = core.FKinBody(M0e, Blist, arm_joints)
# The the transformation between end-effector frame {e} to the chassis frame {b}:
Teb = (core.TransInv(T0e)).dot(core.TransInv(Tb0))
# Calculate the feedforward reference twist:
Vd = core.se3ToVec(
core.MatrixLog6((core.TransInv(Xd)).dot(Xd_next)) / delta_t)
print("Vd: ", Vd)
# Calculate the Adx-1xd matrix:
ADx1xd = core.Adjoint((core.TransInv(X)).dot(Xd))
ADx1xdVd = ADx1xd.dot(Vd)
print("ADx1xdVd: ", ADx1xdVd)
# Calculate the error:
Xerr = core.se3ToVec(core.MatrixLog6((core.TransInv(X)).dot(Xd)))
print("Xerr: ", Xerr)
# Calculate command end effector twist (when the numerical integral of the error is error_integral + Xerr * delta_t):
error_integral += Xerr * delta_t
V = ADx1xdVd + Kp.dot(Xerr) + Ki.dot(error_integral)
print("V: ", V)
# Calculate the arm, body and end-effector Jacobian matrices:
Ja = core.JacobianBody(Blist, arm_joints)
Jb = (core.Adjoint(Teb)).dot(F)
Je = np.concatenate((Jb, Ja), axis=1)
print("Je: ", Je)
# Calculate the wheel and arm joint controls:
Je_inv = np.linalg.pinv(Je)
controls = Je_inv.dot(V)
print("controls: ", controls)
return V, controls, Xerr, error_integral
X_d_next = np.array([[
th.ref_traj[i + 1][0], th.ref_traj[i + 1][1],
th.ref_traj[i + 1][2], th.ref_traj[i + 1][9]
],
[
th.ref_traj[i + 1][3], th.ref_traj[i + 1][4],
th.ref_traj[i + 1][5], th.ref_traj[i + 1][10]
],
[
th.ref_traj[i + 1][6], th.ref_traj[i + 1][7],
th.ref_traj[i + 1][8], th.ref_traj[i + 1][11]
], [0, 0, 0, 1]])
# The error twist Xerr that takes X to Xd in unit time is
# extracted from the 4x4 se(3) matrix [Xerr] = log(X^(−1) Xd)
X_err = mr.se3ToVec(mr.MatrixLog6(np.dot(mr.TransInv(X), X_d)))
# Maintain an estimate of the integral of the error
integral = integral + (X_err * timestep)
# Compute the twist which is necessary to go from X_d to X_d_next
V_d = mr.se3ToVec(
mr.MatrixLog6(np.dot(mr.TransInv(X_d), X_d_next)) / timestep)
# Build the contro law
term1 = np.dot(mr.Adjoint(np.dot(mr.TransInv(X), X_d)), V_d)
term2 = np.dot(K_p, X_err)
term3 = np.dot(K_i, integral)
V = term1 + term2 + term3
# Compute the Jacobian and, using the pseudoinverse, obtain the control signal
def IKinBodyIterate(Blist, M, T, thetalist0, eomg, ev):
"""Computes inverse kinematics in the body frame for an open chain robot
:param Blist: The joint screw axes in the end-effector frame when the
manipulator is at the home position, in the format of a
matrix with axes as the columns
:param M: The home configuration of the end-effector
:param T: The desired end-effector configuration Tsd
:param thetalist0: An initial guess of joint angles that are close to
satisfying Tsd
:param eomg: A small positive tolerance on the end-effector orientation
error. The returned joint angles must give an end-effector
orientation error less than eomg
:param ev: A small positive tolerance on the end-effector linear position
error. The returned joint angles must give an end-effector
position error less than ev
:return thetalist: Joint angles that achieve T within the specified
tolerances,
:return success: A logical value where TRUE means that the function found
a solution and FALSE means that it ran through the set
number of maximum iterations without finding a solution
within the tolerances eomg and ev.
Uses an iterative Newton-Raphson root-finding method.
The maximum number of iterations before the algorithm is terminated has
been hardcoded in as a variable called maxiterations. It is set to 20 at
the start of the function, but can be changed if needed.
Example Input:
Blist = np.array([[0, 0, -1, 2, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 1, 0, 0, 0.1]]).T
M = np.array([[-1, 0, 0, 0],
[ 0, 1, 0, 6],
[ 0, 0, -1, 2],
[ 0, 0, 0, 1]])
T = np.array([[0, 1, 0, -5],
[1, 0, 0, 4],
[0, 0, -1, 1.6858],
[0, 0, 0, 1]])
thetalist0 = np.array([1.5, 2.5, 3])
eomg = 0.01
ev = 0.001
Output:
(np.array([1.57073819, 2.999667, 3.14153913]), True)
"""
thetalist = np.array(thetalist0).copy()
i = 0
maxiterations = 20
Vb = mr.se3ToVec(mr.MatrixLog6(np.dot(mr.TransInv(mr.FKinBody(M, Blist, \
thetalist)), T)))
err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg \
or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev
while err and i < maxiterations:
print("Iterations: ", i)
print("joint vector: ", thetalist)
with open("iterates.csv", "a", newline="") as file:
csv.writer(file).writerow([
thetalist[0], thetalist[1], thetalist[2], thetalist[3],
thetalist[4], thetalist[5]
])
Tsb = mr.FKinBody(M, Blist, thetalist)
print("SE(3) end−effector config: ", Tsb)
print("error twist V_b: ", Vb)
print("angular error magnitude ∣∣omega_b∣∣: ",
np.linalg.norm([Vb[0], Vb[1], Vb[2]]))
print("linear error magnitude ∣∣v_b∣∣: ",
np.linalg.norm([Vb[3], Vb[4], Vb[5]]))
thetalist = thetalist \
+ np.dot(np.linalg.pinv(mr.JacobianBody(Blist, \
thetalist)), Vb)
i = i + 1
Vb \
= mr.se3ToVec(mr.MatrixLog6(np.dot(mr.TransInv(mr.FKinBody(M, Blist, \
thetalist)), T)))
err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg \
or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev
return (thetalist, not err)