Working arm movement to specific pose (x,y,z,roll,pitch,yaw). There are a lot of other changes too.

-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

docker_env/gazebo/Dockerfile

@@ -17,6 +17,7 @@ RUN echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
 RUN apt install -y python-rosdep
 RUN rosdep init
 RUN rosdep update
+RUN apt-get install -y ros-melodic-pid
 # user id 1000 should be the same as the host user, so that you can access files
 # from inside the docker container and also on the host
 RUN useradd -u 1000 rrrobot