This section uses optimization to find control inputs that will drive the robot arm to the ball strike point.
We formulate our motion planning and actuation problem as the minimization of a cost function of our choosing, subject to system constraints.
Solving this problem (using the cvxopt library) generates a reference trajectory of joint angle states (X) and joint velocity inputs (U)
at each time step in the N-step control horizon.
Cost Function:
There are three terms we seek to minimize. The first is the most important, the norm of the error between the current joint angles
and those at the strike point, which are calculated using inverse kinematics based on the end-effector position.
This causes the function to prioritize speed when approaching the strike point, as once the desired angles are achieved the term will
incur no more cost. The second term is a penalty on control action magnitude, ensuring the arm maintains a smooth trajectory.
The third term is a terminal cost constraint, improving the function's ability to reach the desired final position.
Constraints:
The system dynamics are formulated using simple kinematic equations, where the joint angles are determined by the velocity
(approximated as constant through each control step). The joint velocities are similarly constrained using an experimentally-derived
maximum acceleration for each joint. Finally there are constraints on the initial position, to ensure uniformity between trials, and on
the maximum joint angles and velocities.
MPC is an extension of CFTOC, where at each successive time-step we solve the CFTOC problem and implement the first control input determined by the optimization solution. By re-solving the problem each iteration, we effectively close the control loop, utilizing real-time joint angle information to determine our current control action. This is crucial, since any form of model mismatch will inherently cause our predicted state to diverge from reality without a closed-loop controller. We experimented with several different time steps and horizon lengths to find an optimized setup for our problem.