All the equations for controlling an arbitrary trajectory of the right hand as an added prioritized task have been added. It all initializes well in Simulink, Ready to test trajectories and do further debugging