private function called by FDYN
XDD = FDYN2(T, X, FLAG, ROBOT, TORQUEFUN)
Called by FDYN to evaluate the robot velocity and acceleration for
forward dynamics. T is the current time, X = [Q QD]
is the state vector,
ROBOT is the object being integrated, and TORQUEFUN is the string name of
the function to compute joint torques and called as
TAU = TORQUEFUN(T, X)
if not given zero joint torques are assumed.
The result is XDD = [QD QDD]
.