-Removed gravity (temporary, I hope). I was struggling to get inverse dynamics working correctly to provide a torque to compensate for gravity. Eventually, I think we want to have a PID, plus gravity compensation.
-Created a demo world with setpoints that the robot heads to.
-Arm kinematic representation is working.
-PID control of each joint is working to a basic extent. Some segments are more responsive than others.
Still to do:
-reading in SDF should be more robust, and should support all joint types
-better control
-obstacle avoidance in arm movement path
-control with gravity
-arm joint limits, and shortest path to angle (ie considers that theta wraps at 2 pi
