Fix set joint position

[lianghongzhuo]

1. when set joint positions, first step the environment, then enable dynamics. This will fix the robot model that has both dynamic links and static links.
2. minor doc string fix for set_joint_position
3. add a disable_dynamics option in set_to_end function

[stale]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.

[lianghongzhuo]

Could you review this ? @stepjam

[stepjam]

Yes, sorry for delay! Looks good to me, thanks!!