This simulation is based on the WPILib ArmSimulation example which is an implementation of SingleJointedArmSim. We on 3467 are using this to help demystify some of the control challenges associated with this type of robot:

To run, start the simulation mode, open Glass, and once the robot is enabled, update the SmartDashboard setpoints for bottom and top arm angle to see how the arm responds.

Beta for wrist support is available for testing on the double-with-wrist branch.