got joint position controllers working