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

rrrobot_src/src/arm_control/CMakeLists.txt

@@ -149,6 +149,9 @@ include_directories(
 ## The recommended prefix ensures that target names across packages don't collide
 add_executable(${PROJECT_NAME}_node src/arm_controller.cpp src/arm.cpp)
 add_executable(test test/test.cpp src/arm.cpp)
+add_executable(arm_movement_demo test/arm_movement_demo.cpp src/arm.cpp)
+add_executable(pub_sub test/test_pub_sub.cpp)
+add_executable(angles_subscriber test/angles_subscriber.cpp src/arm.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -177,6 +180,30 @@ target_link_libraries(test
   ignition-math4::ignition-math4
 )
 
+target_link_libraries(arm_movement_demo
+  ${catkin_LIBRARIES}
+  ${kdl_parser_LIBRARIES}
+  ${orocos_kdl_LIBRARIES}
+  ${SDF_LIBRARIES}
+  ignition-math4::ignition-math4
+)
+
+target_link_libraries(pub_sub
+  ${catkin_LIBRARIES}
+  ${kdl_parser_LIBRARIES}
+  ${orocos_kdl_LIBRARIES}
+  ${SDF_LIBRARIES}
+  ignition-math4::ignition-math4
+)
+
+target_link_libraries(angles_subscriber
+  ${catkin_LIBRARIES}
+  ${kdl_parser_LIBRARIES}
+  ${orocos_kdl_LIBRARIES}
+  ${SDF_LIBRARIES}
+  ignition-math4::ignition-math4
+)
+
 #############
 ## Install ##
 #############