Update README and example cmake project

README.md

@@ -85,27 +85,45 @@ $ make check
 
 ## Running Examples
 
-The `/examples` directory contains example projects that demonstrate how to include GTDynamics in your application. To run an example, ensure that the `CMAKE_PREFIX_PATH` is set to the GTDynamics install directory.
+The `examples` directory contains various full example projects demonstrating the use of GTDynamics for various robotic applications.
 
-1. Navigate to the example's subdirectory and create a build directory. e.g.
+We recommend going through the examples to get a better understanding of how to use GTDynamics for your own use cases.
+
+*NOTE* The examples are made to run within the GTDynamics source folder and are not standalone projects.
+
+Run these examples with
 ```sh
-cd GTDynamics/examples/example_forward_dynamics
-mkdir build; cd build
+$ make example_XXX.run
 ```
+where `XXX` corresponds to a folder name in `examples`.  For example, `make example_forward_dynamics.run`.
+
+## Including GTDynamics With CMake
+
+The `examples/cmake_project_example` directory contains an example CMake-based project that demonstrates how to include GTDynamics in your application.
+
+Use this as a template when you want to set up your own project that uses GTDynamics (e.g. separate git repo, ROS, personal libraries, etc).
+
+To build the project:
+
+1. Navigate to the example's subdirectory and create a build directory. e.g.
+    ```sh
+    cd GTDynamics/examples/cmake_project_example
+    mkdir build; cd build
+    ```
 
 2. Make the example.
 
-If GTDynamics was installed to `~/JohnDoe/gtdynamics_install`, then run the cmake command with:
+    If GTDynamics was installed to `~/JohnDoe/GTDynamics/install`, then run the cmake command with:
 
-```sh
-cmake -DCMAKE_PREFIX_PATH=~/JohnDoe/gtdynamics_install ..
-make
-```
+    ```sh
+    cmake -DCMAKE_PREFIX_PATH=~/JohnDoe/GTDynamics/install ..
+    make
+    ```
 
 3. Run the example!
-```sh
-./exec
-```
+    ```sh
+    ./example
+    ```
 
 ## Python Wrapper (Recommended use case)
 

examples/cmake_project_example/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.0)
+project(example C CXX)
+
+# gtsam
+find_package(GTSAM REQUIRED)
+
+# gtdynamics
+find_path(gtdynamics_include gtdynamics)
+find_library(gtdynamics_lib gtdynamics)
+# these next 3 lines are temporary until we create a proper cmake module for gtdynamics.
+set(SDFormat_VERSION 12)
+find_package(sdformat${SDFormat_VERSION} REQUIRED)
+list(APPEND gtdynamics_lib ${SDFormat_LIBRARIES})
+
+# Build Executable
+add_executable(${PROJECT_NAME} main.cpp)
+target_link_libraries(${PROJECT_NAME} PUBLIC ${gtdynamics_lib} gtsam)
+target_include_directories(${PROJECT_NAME} PUBLIC ${gtdynamics_include})

examples/cmake_project_example/main.cpp

@@ -0,0 +1,52 @@
+/**
+ * @file main.cpp
+ * @brief gtdynamics cmake Project Example
+ * This code demonstrates a sample project that imports gtdynamics with cmake.
+ * This cpp file should be the same as `example_forward_dynamics/main.cpp`.
+ */
+
+#include <gtdynamics/dynamics/DynamicsGraph.h>
+#include <gtdynamics/universal_robot/Robot.h>
+#include <gtdynamics/universal_robot/sdf.h>
+#include <gtdynamics/utils/Initializer.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+
+using namespace gtdynamics;
+int main(int argc, char** argv) {
+  // Load the robot
+  auto simple_rr = CreateRobotFromFile(
+      kSdfPath + std::string("test/simple_rr.sdf"), "simple_rr_sdf");
+  simple_rr.print();
+
+  gtsam::Vector3 gravity(0, 0, -9.8);
+
+  // Build a nonlinear factor graph of kinodynamics constraints.
+  auto graph_builder = DynamicsGraph(gravity);
+  auto kdfg = graph_builder.dynamicsFactorGraph(simple_rr,
+                                                0  // timestep
+  );
+
+  // Specify the forward dynamics priors and add them to the factor graph.
+  gtsam::Values known_values;
+  for (auto&& joint : simple_rr.joints()) {
+    int j = joint->id();
+    InsertJointAngle(&known_values, j, 0, 0.0);
+    InsertJointVel(&known_values, j, 0, 0.0);
+    InsertTorque(&known_values, j, 0, 0.0);
+  }
+  auto fd_priors =
+      graph_builder.forwardDynamicsPriors(simple_rr, 0, known_values);
+  kdfg.add(fd_priors);
+
+  // Initialize solution.
+  Initializer initializer;
+  auto init_values = initializer.ZeroValues(simple_rr, 0);
+
+  // Compute the forward dynamics.
+  gtsam::LevenbergMarquardtOptimizer optimizer(kdfg, init_values);
+  gtsam::Values results = optimizer.optimize();
+  graph_builder.printValues(results);
+
+  return 0;
+}