Design Brainstorming
PCB Design
End-Effector Design
Perception Subsystem
Planning Subsystem
Controls Subsystem
Miscellaneous
MPC Brainstorming
Deciding the Controller and Formulating the Optimal Control Problem
After surveying the possible controls algorithms that could be used for this system, a Model Predictive Controller (MPC) was decided as the best controller because of our system constraints and goals, changing system dynamics, and that MPC can take multiple inputs. Using classic manipulator dynamics, the system dynamics were determined. Constraints were determined based on literature research and system requirements. For the objective function, a tracking objective was chosen, minimizing the error along a desired trajectory while meeting constraints. Figure 16 shows the Optimal Control Problem.
Implementing the Dynamics Modeling of the Kinova Gen3 Arm
The dynamics function of the Kinova Gen3 Arm was implemented using the Julia Language and the RigidBodyDynamics.jl library. This library parses the URDF of our model and builds a joint-linkage tree, storing the mass and inertias. Given the joint positions, joint velocities, external wrenches, and input torques, the library has dynamics functions that can calculate the joint accelerations. Figure 17 shows the currently implemented dynamics function.
Implementing the MPC
Using the RobotDynamics.jl, TrajectoryOptimization.jl, and Altro.jl, the MPC was implemented, organizing a trajectory of desired states and desired inputs over time, setting up the Tracking Objective function, and setting up the System Constraints defined above in Figure 16. The most challenging part of setting the MPC up was providing a good initial guess for input torques for the solver to converge. The best initial guess was providing gravity compensation torques for the initial position of the manipulator in the trajectory. Figure 18 shows the implementation of the objective function and joint position and velocity constraints. Figure 19 shows an offline simulation of the MPC output, which had a very low tracking error (less than 0.001 mm and less than 0.5 degrees error of end-effector pose).
Integrating the MPC into ROS
RobotOS.jl was used to integrate the MPC into ROS within the Julia Language, and is a wrapper for PyROS, allowing for the node to subscribe and publish messages within ROS. Two nodes were written as part of the controller: an MPC node to solve the optimal control problem given the current state, desired trajectory and system constraints, and a Controller node which sends the appropriate torques to the effort controllers in simulation or hardware.
Integrating the Motion Planner and Simulating in Gazebo
After writing the ROS nodes, the MPC was integrated with the Motion Planner subsystem and with the Gazebo simulation and joint effort controllers at a rate of 20 Hz (will increase later). This enabled real-time simulation and testing of the MPC, which revealed challenges with getting the MPC to converge on time.
Implementing the PD Impedance Tracking Controller
Since there were issues with getting the MPC to converge, a PD Impedance Tracking Controller was developed in parallel. Using the RobotDynamics.jl, the error in desired joint positions and velocities were given to the controller to determine the desired joint accelerations. Using the desired accelerations, the desired torques were calculated using inverse dynamics within the library, which uses the Recursive Newton Euler Algorithm. These torques are then provided to the effort controllers. Figure 20 shows the real-time simulation of the PD Impedance Tracking Controller following a straight-line trajectory in Gazebo.