Robotic Manipulation
youBot picking up and placing down a block in a specified spot (feedback gains: Kp = 0.6 and Ki = 0.001)
The error due to the initial offset is quickly corrected by the PI controller. The cube is picked up and placed exactly at its goal configuration without observable error.
youBot picking up a block from a new spot and placing down the block, also, in a new spot (feedback gains: Kp = 0.4 and Ki = 0.001)
Like the simulation above, the error due to the initial offset is quickly corrected by the PI controller. The cube is picked up and placed exactly at its goal configuration without observable error.
youBot overshoots its trajectory before reaching steady-state (feedback gains: Kp = 1.0 and Ki = 1.5)
There is observable overshoot during the first trajectory on the robot's path. The robot arm oscillates wildly in space before it reaches steady-state and begins to follow the path with little to no observable error. Though there is overshoot in the first segment of the path, the robot is able to pick up and place the cube in its goal configuration without error.
mISSION
The goal of this project is to use the youBot, an omniwheel, mobile robot with a 5R arm attached to its chassis, to pick up a cube from its initial position and place the cube at its goal position. I will generate the trajectory of the end-effector and then apply feedforward/back control to keep the end-effector on the planned path.
Overview
The project is broken down into three major steps.
Compute the configuration of the robot at the next time step, obtained from odometry
Generate the path the end-effector takes to get to each goal configuration
Correct errors in the position of the end-effector using feedback control
Putting it all together, the program first generates the path the end-effector should take. Then, it loops through each configuration on the reference trajectory and computes the error between the current end-effector configuration and reference one. Using the error and its integral (sum over time), adjusted wheel and joint speeds are calculated and used to determine the configuration at the next time step.
Implementation
The NextState function simulates the kinematics of a youBot robot. The function takes as input the current configuration of the robot, including chassis and arm joint angles, along with control inputs representing wheel speeds and arm joint speeds. It performs a first-order Euler integration to compute the robot's configuration at the next time step, accounting for speed limits specified by the maximum angular speed parameter. Notably, the code incorporates odometry calculations to update the chassis configuration based on the wheel displacements. The MobileBaseConfig function defines the transformation matrix for the mobile base configuration. In addition, the implementation ensure that joint and wheel speeds do not exceed the specified speed limits.
def NextState(thetalist, dthetalist, dt, wmax):
# Apply speed limits
for i, dtheta in enumerate(dthetalist):
if dtheta > wmax:
dthetalist[i] = wmax
elif dtheta < -wmax:
dthetalist[i] = -wmax
nextJoint = thetalist[3:8] + dt*dthetalist[4:]
nextWheel = thetalist[8:] + dt*dthetalist[:4]
# Calculate new chasis config using odometry
wheelDisp = nextWheel - thetalist[8:]
lplusw = 0.385 # length + width of mobile base
r = 0.0475 # radius of wheels
F = r/4 * np.array([[-1/lplusw, 1/lplusw, 1/lplusw, -1/lplusw],
[1, 1, 1, 1],
[-1, 1, -1, 1]])
Vb6 = np.hstack(([0, 0], F @ wheelDisp, 0))
Tb1 = mr.MatrixExp6(mr.VecTose3(Vb6))
Ts1 = MobileBaseConfig(thetalist[:3]) @ Tb1
nextChasis = np.array([np.arccos(Ts1[0, 0]), Ts1[0, 3], Ts1[1, 3]])
return np.hstack((nextChasis, nextJoint, nextWheel))
The TrajectoryGenerator function generates a reference trajectory for the end-effector of a youBot robot, encompassing eight concatenated trajectory segments. These segments guide the end-effector through a sequence of key configurations, including initial and final standoff configurations, grasp configurations, and transitional configurations. The function takes as input the initial configuration of the end-effector (Tsei), the initial and final configurations of a cube (Tsci and Tscf), the end-effector's grasp and standoff configurations relative to the cube (Tceg and Tces), and the desired number of trajectory reference configurations per 0.01 seconds (k). The trajectory is generated by calculating the necessary time duration for each segment based on the distance and angle between the relevant configurations, ensuring that the duration is not too short to require unreasonable joint and wheel speeds. Gripper opening and closing times are also considered. The resulting trajectory is represented as a list of configurations, each including rotation and translation components along with the gripper state (0 for open, 1 for closed).
def TrajectoryGenerator(Tsei, Tsci, Tscf, Tceg, Tces, k):
# Configuration targets
T1 = Tsci @ Tces
T2 = Tsci @ Tceg
T5 = Tscf @ Tces
T6 = Tscf @ Tceg
eeTraj = [Tsei, T1, T2, T2, T1, T5, T6, T6, T5]
# Time needed to traverse each segment
gripTime = 0.9 # amount of time it takes gripper to open/close
t1 = SegmentDuration(eeTraj[0], eeTraj[1])
t2 = SegmentDuration(eeTraj[1], eeTraj[2])
t3 = gripTime # gripper closing
t4 = SegmentDuration(eeTraj[3], eeTraj[4])
t5 = SegmentDuration(eeTraj[4], eeTraj[5])
t6 = SegmentDuration(eeTraj[5], eeTraj[6])
t7 = gripTime # gripper opening
t8 = SegmentDuration(eeTraj[7], eeTraj[8])
timeVec = [t1, t2, t3, t4, t5, t6, t7, t8]
isClosed = 0 # is gripper closed?
trajTotal = [] # list of [r11, r12, r13, r21, r22, r23, r31, r32, r33, px, py, pz, gripperState]
for i in range(8):
segment = mr.CartesianTrajectory(eeTraj[i], eeTraj[i+1], timeVec[i], int(timeVec[i]*100*k), 5)
if i == 2 or i == 6:
isClosed ^= 1 # reverse gripper state in segments 3 and 7
for config in segment:
trajTotal.append([config[0][0], config[0][1], config[0][2],
config[1][0], config[1][1], config[1][2],
config[2][0], config[2][1], config[2][2],
config[0][3], config[1][3], config[2][3],
isClosed])
return trajTotal
The FeedBackControl function implements a kinematic task-space control law that combines feedforward and feedback components to regulate the motion of the end-effector. The function takes into account the current joint configurations, the actual end-effector configuration, and the desired end-effector configurations at the current and next timesteps. Utilizing the error in end-effector pose and a numerical integral of this error, the function computes the feedforward reference twist that aligns with the desired motion. Incorporating proportional and integral gain matrices, the function generates a control law that commands the end-effector twist. This twist is then transformed into the corresponding wheel and arm joint speeds using the pseudoinverse of the combined Jacobian matrix, considering both the arm and mobile base. The resulting control signals enable the youBot to perform precise and dynamic movements in response to the specified trajectory, while considering the inherent kinematic constraints and dynamics of the robotic system.
def FeedBackControl(thetalist, X, Xd, Xdnext, Kp, Ki, intXerr, dt):
Xerr = mr.se3ToVec(mr.MatrixLog6(mr.TransInv(X) @ Xd))
intXerr = intXerr + dt*Xerr
Vd = 1/dt*mr.se3ToVec(mr.MatrixLog6(mr.TransInv(Xd) @ Xdnext))
V = mr.Adjoint(mr.TransInv(X) @ Xd) @ Vd + Kp @ Xerr + Ki @ intXerr
Tb0 = np.array([[ 1, 0, 0, 0.1662], # offset of chasis and arm base
[ 0, 1, 0, 0],
[ 0, 0, 1, 0.0026],
[ 0, 0, 0, 1]])
M0e = np.array([[ 1, 0, 0, 0.033], # home config
[ 0, 1, 0, 0],
[ 0, 0, 1, 0.6546],
[ 0, 0, 0, 1]])
Blist = np.array([[0, 0, 1, 0, 0.033, 0], # screw axes of arm joints
[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
lplusw = 0.385 # length + width of mobile base
r = 0.0475 # radius of wheels
F = r/4 * np.array([[0, 0, 0, 0],
[0, 0, 0, 0],
[-1/lplusw, 1/lplusw, 1/lplusw, -1/lplusw],
[1, 1, 1, 1],
[-1, 1, -1, 1],
[0, 0, 0, 0]])
T0e = mr.FKinBody(M0e, Blist, thetalist[3:])
Ja = mr.JacobianBody(Blist, thetalist[3:]) # Jacobian of arm
Jb = mr.Adjoint(mr.TransInv(T0e) @ mr.TransInv(Tb0)) @ F # Jacobian of base
Je = np.hstack((Jb, Ja)) # combined Jacobian
udtheta = np.linalg.pinv(Je, rcond=1e-3) @ V
return [udtheta, Xerr, intXerr]
Putting it all together, the code below is the implementation for trajectory tracking and control of the youBot mobile manipulator. The script can be executed from the command line with different arguments (best, overshoot, or newTask) to evaluate the robot's performance under different control scenarios.
The ReconstructTransMat function is defined to convert a configuration vector into a 4x4 transformation matrix. Additionally, CubeConfig constructs the configuration matrix for the cube based on a given configuration vector. The script initializes the robot and trajectory parameters, including the initial and final cube configurations, gains for the proportional (P) and integral (I) controllers, and the desired trajectory. It then iterates through the trajectory, applying feedback control at each step. The resulting joint configurations and errors are stored in arrays for further analysis and visualization.
Kp = 0.6*np.eye(6) # P gain
Ki = 0.001*np.eye(6) # I gain
Tsci = CubeConfig([1, 0, 0]) # initial cube config
Tscf = CubeConfig([0, -1, -np.pi/2]) # final cube config
# Generate reference trajectory
refTraj = TrajectoryGenerator(Tsei, Tsci, Tscf, Tceg, Tces, k)
# Apply feedforward and feedback control based on actual config
for i in range(len(refTraj)-1):
actualTse = MobileBaseConfig(currentConfig[:3]) @ Tb0 @ mr.FKinBody(M0e, Blist, currentConfig[3:8])
Tsed = ReconstructTransMat(refTraj[i][:-1])
Tsednext = ReconstructTransMat(refTraj[i+1][:-1])
[controls, Xerr, intXerr] = FeedBackControl(currentConfig[:8], actualTse, Tsed, Tsednext, Kp, Ki, intXerr, dt)
currentConfig = np.hstack((NextState(currentConfig[:-1], controls, dt, 2), refTraj[i+1][-1]))
if i != 0 and i % k == 0:
configMat.append(currentConfig)
XerrMat.append(Xerr)
Results
Simulation 1 Error Plot of End Effector
Simulation 2 Error Plot of End Effector
Simulation 3 Error Plot of End Effector
Overall, all the simulations looks good. The first simulation quickly corrects the error caused by the offset initial configuration. The second simulation works similarly to the first simulation, just with different initial and final cube configurations. The third simulation demonstrates oscillation in the first few seconds, but is able to achieve steady-state motion. All simulations pick up and place the cube in the correct position and orientation.
