Finished example with Computer torque control and PD + gravity compensation