def forward_kinematics(self):
T_0e = mr.FKinBody(M=self.M_0e, Blist=self.Blist, thetalist=self.m_q)
T_sb = np.array([[np.cos(self.c_q[0]), -np.sin(self.c_q[0]), 0, self.c_q[1]],
[np.sin(self.c_q[0]), np.cos(self.c_q[0]), 0, self.c_q[2]],
[0, 0, 1, 0.0963],
[0, 0, 0, 1]])
T_se = np.linalg.multi_dot((T_sb, self.T_b0, T_0e))
return T_se
def compute_J_e(self):
T_0e = mr.FKinBody(M=self.M_0e, Blist=self.Blist, thetalist=self.m_q)
J_base = np.dot(
mr.Adjoint(np.dot(mr.TransInv(T_0e), mr.TransInv(self.T_b0))),
self.F_6)
J_arm = mr.JacobianBody(Blist=self.Blist, thetalist=self.m_q)
J_e = np.append(J_base, J_arm, axis=1)
return J_e
def main():
""" The project's main function.
This function Generates the robot's trajectory using the TrajectoryGenerator, NextState and FeedbackControl
functions.
Input:
None
Return:
A saved csv file represents the N configurations of the end-effector along the entire
concatenated eight-segment reference trajectory, including the gripper state (a 13-vector)
A saved csv file represents the Xerr (a 6-vector)
A saved plot of the Xerr changing with time
A saved log file
"""
########## Initialization ##########
# Initialization of configuration matrices:
# The initial configuration of the robot
# (chassis phi, chassis x, chassis y, J1, J2, J3, J4, J5, W1, W2, W3, W4, gripper state)
# The end-effector has at least 30 degrees of orientation error and 0.2m of position error:
initial_config = np.array([0.1, -0.2, 0, 0, 0, 0.2, -1.6, 0, 0, 0, 0, 0, 0])
# The initial configuration of the end-effector in the reference trajectory:
Tse_initial = np.array([[ 0, 0, 1, 0],
[ 0, 1, 0, 0],
[ -1, 0, 0, 0.5],
[ 0, 0, 0, 1]])
# The cube's initial configuration:
Tsc_initial = np.array([[1, 0, 0, 1],
[0, 1, 0, 0],
[0, 0, 1, 0.025],
[0, 0, 0, 1]])
# The cube's desired final configuration:
Tsc_goal = np.array([[ 0, 1, 0, 0],
[ -1, 0, 0, -1],
[ 0, 0, 1, 0.025],
[ 0, 0, 0, 1]])
# The end-effector's configuration relative to the cube when it is grasping the cube
# (the two frames located in the same coordinates, rotated about the y axis):
Tce_grasp = np.array([[ -1/np.sqrt(2), 0, 1/np.sqrt(2), 0],
[ 0, 1, 0, 0],
[ -1/np.sqrt(2), 0, -1/np.sqrt(2), 0],
[ 0, 0, 0, 1]])
# The end-effector's standoff configuration above the cube, before and after grasping, relative
# to the cube (the {e} frame located 0.1m above the {c} frame, rotated about the y axis):
Tce_standoff = np.array([[ -1/np.sqrt(2), 0, 1/np.sqrt(2), 0],
[ 0, 1, 0, 0],
[ -1/np.sqrt(2), 0, -1/np.sqrt(2), 0.1],
[ 0, 0, 0, 1]])
# 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
# Initialization of feedback control constants:
kp_gain = 20 # The kp gain
ki_gain = 5 # The ki gain
Kp = np.identity(6) * kp_gain # The P gain matrix
Ki = np.identity(6) * ki_gain # The I gain matrix
# Restrictions on the speeds vector:
max_ang_speed = 10
# Initialization of simulation constants:
k = 1 # The number of trajectory reference configurations per 0.01 seconds
delta_t = 0.01 # Time step [sec]
t_total = 16 # Simulation run time [sec]
iteration = int(t_total/delta_t) # Number of iterations
# Initialization of variable lists:
config_array = np.zeros((iteration, 13))
Xerr_array = np.zeros((iteration, 6))
error_integral = np.zeros(6)
# Add the initial configuration to the config_array:
config_array[0] = initial_config
########## Calculating Configurations ##########
# Trajectory Generation:
logging.info('Generating trajectory')
trajectory = TrajectoryGenerator(Tse_initial, Tsc_initial, Tsc_goal, Tce_grasp, Tce_standoff, k)
for i in range(1, iteration-1):
current_config = config_array[i-1,:]
# Define transformation matrices to hepl find the current, desired and next desired position:
Tsb = np.array([[cos(current_config[0]), -sin(current_config[0]), 0, current_config[1]],
[sin(current_config[0]), cos(current_config[0]), 0, current_config[2]],
[ 0, 0, 1, 0.0963],
[ 0, 0, 0, 1]])
T0e = core.FKinBody(M0e, Blist, current_config[3:8])
Tbe = Tb0.dot(T0e)
# Define current, desired and next desired position:
X = Tsb.dot(Tbe)
print("X: ", X)
Xd = np.array([[trajectory[i][0], trajectory[i][1], trajectory[i][2], trajectory[i][9]],
[trajectory[i][3], trajectory[i][4], trajectory[i][5], trajectory[i][10]],
[trajectory[i][6], trajectory[i][7], trajectory[i][8], trajectory[i][11]],
[ 0, 0, 0, 1]])
print("Xd: ", Xd)
Xd_next = np.array([[trajectory[i+1][0], trajectory[i+1][1], trajectory[i+1][2], trajectory[i+1][9]],
[trajectory[i+1][3], trajectory[i+1][4], trajectory[i+1][5], trajectory[i+1][10]],
[trajectory[i+1][6], trajectory[i+1][7], trajectory[i+1][8], trajectory[i+1][11]],
[ 0, 0, 0, 1]])
print("Xd_next: ", Xd_next)
# Calculate the control law:
V, controls, Xerr, error_integral = FeedbackControl(X, Xd, Xd_next, Kp, Ki, delta_t, current_config, error_integral)
controls_flipped = np.concatenate((controls[4:9], controls[:4]), axis=None)
# Calculate the next configuration:
current_config = NextState(current_config[:12], controls_flipped, delta_t, max_ang_speed)
config_array[i] = np.concatenate((current_config, trajectory[i][12]), axis=None)
# Calculate the error:
Xerr_array[i-1] = Xerr
# Save the configurations as a csv file:
logging.info('Generating trajectory csv file')
with open("configurations.csv","w+") as my_csv1:
csvWriter = csv.writer(my_csv1, delimiter=',')
csvWriter.writerows(config_array)
# Save the error as a csv file:
logging.info('Generating error csv file')
with open("Xerr.csv","w+") as my_csv2:
csvWriter = csv.writer(my_csv2, delimiter=',')
csvWriter.writerows(Xerr_array)
# Plot the error as function of time:
logging.info('plotting error data')
t_traj = np.linspace(1, 16, 1600)
print("Xerr_array: ", Xerr_array)
print("Xerr_array.shape: ", Xerr_array.shape)
plt.figure()
plt.plot(t_traj, Xerr_array[:,0], label='Xerr[0]')
plt.plot(t_traj, Xerr_array[:,1], label='Xerr[1]')
plt.plot(t_traj, Xerr_array[:,2], label='Xerr[2]')
plt.plot(t_traj, Xerr_array[:,3], label='Xerr[3]')
plt.plot(t_traj, Xerr_array[:,4], label='Xerr[4]')
plt.plot(t_traj, Xerr_array[:,5], label='Xerr[5]')
plt.title(f'Xerr, kp={kp_gain}, ki={ki_gain}')
plt.xlabel('Time (s)')
plt.xlim([1, 16])
plt.ylabel('Error')
plt.legend(loc="best")
plt.grid()
plt.savefig(f'Xerr,kp={kp_gain},ki={ki_gain}.png')
plt.show()
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
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)