Working on a simple PID control system for a 2 robot system with 4 degrees of freedom on each robot. We know we have a singularity when both robots are at [0 0 0 0]. No matter what values are going into our Drone Plant it seems to be outputting [0 0 0 0] every time. We tried including initial conditions but after the first iteration, it goes back to the Drone Plant output of [0 0 0 0]. We are looking for any advice, solutions, or troubleshooting techniques...anything. Thank you for your time.
The error:
The error message we are receiving is- Derivative input 1 of 'TwoDroneMathworks/Drone 1 Plant/transfer Fcn' at time 0.05 is Inf or NaN. Stopping simulation. There may be a singularity in the solution. If not, try reducing the step size (either by reducing the fixed step size or by tightening the error tolerances) We have tried utilizing resources provided by MathWorks for troubleshooting this specific error without any luck.
Break down of the script:
We have a trajectory signal that is generating our desired trajectory for the cluster which goes into our PID controller. We have an inverse jacobian that converts commanded cluster space velocity into individual robot velocities commands. We plug that into a simple first order plant and integrate to get position. Then our kinematic block converts the individual robot positions into a position of the cluster. We have tested both Matlab scripts (Inverse Jacobian and Forward Kinematics) independently multiple times and believe they are functioning as expected.
