Added painting function and forward kinematics