Fixed getPoseViz, added function for replacing joint position in a sensor_msgs::JointState msg