add both q and kinsol as input arguments in drakeFunction.kinematics