problems with Forward KInematics when using operational space scheme.