diff --git a/examples/example_a1_walking/CMakeLists.txt b/examples/example_a1_walking/CMakeLists.txt
index e2af57c2..1c9f98b0 100644
--- a/examples/example_a1_walking/CMakeLists.txt
+++ b/examples/example_a1_walking/CMakeLists.txt
@@ -9,8 +9,20 @@ add_executable(${WALK_FORWARD} main.cpp)
 target_link_libraries(${WALK_FORWARD} PUBLIC gtdynamics)
 target_include_directories(${WALK_FORWARD} PUBLIC ${CMAKE_PREFIX_PATH}/include)
 
+# Walk forward one meter.
+set(WALK_FORWARD_COMBINED ${PROJECT_NAME}_forward_combined)
+add_executable(${WALK_FORWARD_COMBINED} main_combined.cpp)
+target_link_libraries(${WALK_FORWARD_COMBINED} PUBLIC gtdynamics)
+target_include_directories(${WALK_FORWARD_COMBINED} PUBLIC ${CMAKE_PREFIX_PATH}/include)
+
 add_custom_target(
   ${WALK_FORWARD}.run
   COMMAND ./${WALK_FORWARD}
   DEPENDS ${WALK_FORWARD}
   WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/examples/${PROJECT_NAME})
+
+  add_custom_target(
+  ${WALK_FORWARD_COMBINED}.run
+  COMMAND ./${WALK_FORWARD_COMBINED}
+  DEPENDS ${WALK_FORWARD_COMBINED}
+  WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/examples/${PROJECT_NAME})
\ No newline at end of file
diff --git a/examples/example_a1_walking/main.cpp b/examples/example_a1_walking/main.cpp
index 54672ce6..1283ff77 100644
--- a/examples/example_a1_walking/main.cpp
+++ b/examples/example_a1_walking/main.cpp
@@ -14,6 +14,8 @@
 #include <gtdynamics/factors/ObjectiveFactors.h>
 #include <gtdynamics/universal_robot/sdf.h>
 #include <gtdynamics/utils/Trajectory.h>
+#include <gtdynamics/dynamics/ChainDynamicsGraph.h>
+#include <gtdynamics/utils/ChainInitializer.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/base/timing.h>
@@ -50,33 +52,174 @@ Trajectory getTrajectory(const Robot& robot, size_t repeat) {
   auto RRFL = std::make_shared<FootContactConstraintSpec>(rrfl, contact_in_com);
 
   //FootContactVector states = {noRL, noRR, noFR, noFL};
-  FootContactVector states = {stationary, RRFL, stationary, RLFR};
-  std::vector<size_t> phase_lengths = {1,5,1,5};
+  FootContactVector states = {RRFL,stationary, RLFR, stationary};
+  std::vector<size_t> phase_lengths = {25, 5, 25, 5};
 
   WalkCycle walk_cycle(states, phase_lengths);
 
   return Trajectory(walk_cycle, repeat);
 }
 
-int main(int argc, char** argv) {
+int massGraph(){
 
-  gttic_(start); //need to build gtsam with timing for this
+  gttic_(start1); //need to build gtsam with timing for this
 
   // Load Unitree A1 robot urdf.
   auto robot =
-      CreateRobotFromFile(std::string("/home/dan/.local/lib/python3.8/site-packages/pybullet_data/a1/a1.urdf"), "a1");
+      CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1");
+
+  double sigma_dynamics = 1e-3;    // std of dynamics constraints.
+  double sigma_objectives = 1e-3;  // std of additional objectives.
+
+  // Noise models.
+  auto dynamics_model_6 = Isotropic::Sigma(6, sigma_dynamics),
+       dynamics_model_1 = Isotropic::Sigma(1, sigma_dynamics),
+       objectives_model_6 = Isotropic::Sigma(6, sigma_objectives),
+       objectives_model_1 = Isotropic::Sigma(1, sigma_objectives);
+
+  // Env parameters.
+  gtsam::Vector3 gravity(0, 0, -9.8);
+  double mu = 1.0;
+
+  //OptimizerSetting opt(sigma_dynamics);
+   OptimizerSetting opt(1e-3, 1e-3, 1e-3, 1e-2);
+  DynamicsGraph graph_builder(opt, gravity);
+
+  // Create the trajectory, 1 walk cycle.
+  auto trajectory = getTrajectory(robot, 1);
 
+  // Create multi-phase trajectory factor graph
+  auto collocation = CollocationScheme::Euler;
+  auto graph =
+      trajectory.multiPhaseFactorGraph(robot, graph_builder, collocation, mu);
 
+  // Build the objective factors.
+  double ground_height = 1.0;
+  const Point3 step(0.25, 0, 0);
+  gtsam::NonlinearFactorGraph objectives =
+      trajectory.contactPointObjectives(robot, Isotropic::Sigma(3, 1e-6), step, ground_height);
+
+  // Get final time step.
+  int K = trajectory.getEndTimeStep(trajectory.numPhases() - 1);
+
+  // Add body goal objectives to the factor graph.
+  auto base_link = robot.link("trunk");
+  for (int k = 0; k <= K; k++) {
+    objectives.add(
+        LinkObjectives(base_link->id(), k)
+            .pose(Pose3(Rot3(), Point3(0, 0, 0.4)), Isotropic::Sigma(6, 1e-2))
+            .twist(gtsam::Z_6x1, Isotropic::Sigma(6, 5e-2)));
+  }
+
+  // Add link and joint boundary conditions to FG.
+  trajectory.addBoundaryConditions(&objectives, robot, dynamics_model_6,
+                                   dynamics_model_6, objectives_model_6,
+                                   objectives_model_1, objectives_model_1);
+
+  // Constrain all Phase keys to have duration of 1 /240.
+  const double desired_dt = 1. / 20;
+  trajectory.addIntegrationTimeFactors(&objectives, desired_dt, 1e-30);
+
+  // Add min torque objectives.
+  //trajectory.addMinimumTorqueFactors(&objectives, robot, Unit::Create(1));
+
+  // Add prior on A1 lower joint angles
+  auto prior_model_angles = Isotropic::Sigma(1, 1e-2);
+  auto prior_model_hip = Isotropic::Sigma(1, 1e-2);
+  for (auto&& joint : robot.joints())
+    if (joint->name().find("lower") < 100)
+      for (int k = 0; k <= K; k++) {
+        //std::cout << joint->name() << joint->name().find("lower") << std::endl;
+        objectives.add(JointObjectives(joint->id(), k).angle(-1.4, prior_model_angles));
+      }
+
+  // Add prior on A1 lower joint angles
+  /*for (auto&& joint : robot.joints())
+    if (joint->name().find("upper") < 100)
+      for (int k = 0; k <= K; k++) {
+        //std::cout << joint->name() << joint->name().find("lower") << std::endl;
+        objectives.add(JointObjectives(joint->id(), k).torque(0.0, prior_model_torques));
+      }
+
+    // Add prior on A1 lower joint angles
+  for (auto&& joint : robot.joints())
+    if (joint->name().find("hip") < 100)
+      for (int k = 0; k <= K; k++) {
+        //std::cout << joint->name() << joint->name().find("lower") << std::endl;
+        objectives.add(JointObjectives(joint->id(), k).torque(0.0, prior_model_torques));
+      } */
+
+  // Add prior on A1 hip joint angles
+  //prior_model = Isotropic::Sigma(1, 1e-3);
+  for (auto&& joint : robot.joints())
+    if (joint->name().find("hip") < 100)
+      for (int k = 0; k <= K; k++) {
+        //std::cout << joint->name() << joint->name().find("hip") << std::endl;
+        objectives.add(JointObjectives(joint->id(), k).angle(0.0, prior_model_hip));
+      }
+ 
+   // Add prior on A1 hip joint angles
+  //prior_model = Isotropic::Sigma(1, 1e-3);
+  for (auto&& joint : robot.joints())
+    if (joint->name().find("upper") < 100)
+      for (int k = 0; k <= K; k++) {
+        //std::cout << joint->name() << joint->name().find("hip") << std::endl;
+        objectives.add(JointObjectives(joint->id(), k).angle(0.7, prior_model_angles));
+      }
+  // Add objectives to factor graph.
+   graph.add(objectives);
+
+  // Initialize solution.
+  double gaussian_noise = 1e-30;
+ Initializer initializer;
+  gtsam::Values init_vals =
+      trajectory.multiPhaseInitialValues(robot, initializer, gaussian_noise, desired_dt);
+
+
+  gttic_(optimization1);
+
+  std::cout << "graph size = " << graph.size() << std::endl;
+  std::cout << "graph keys size = " << graph.keys().size() << std::endl;
+  std::cout << "init vals size = " << init_vals.size() << std::endl;
+  
+  // Optimize!
+  gtsam::LevenbergMarquardtParams params;
+  //params.setVerbosityLM("SUMMARY");
+  params.setlambdaInitial(1e10);
+  params.setlambdaLowerBound(1e-7);
+  params.setlambdaUpperBound(1e10);
+  params.setAbsoluteErrorTol(1.0);
+  gtsam::LevenbergMarquardtOptimizer optimizer(graph, init_vals, params);
+  auto results = optimizer.optimize();
+
+  gttoc_(optimization1);
+  gttoc_(start1);
+
+  //gtsam::tictoc_print_(); //need to build gtsam with timing for this
+  // Write results to traj file
+  trajectory.writeToFile(robot, "a1_traj_DG_mass.csv", results);
+
+  return 0;
+}
+
+int oldGraph(){
+
+  gttic_(start2); //need to build gtsam with timing for this
+
+  // Load Unitree A1 robot urdf.
+  auto robot =
+      CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1");
+
+  // set masses and inertias
   for (auto&& link : robot.links()) {
-    if (link->name().find("trunk") > 100) {
-        //link->setMassValue(0);
-        //link->setInertiaZero();
-        std::cout << link->name() << std::endl;
+    if (link->name().find("trunk") == std::string::npos) {
+      link->setMass(0.0);
+      link->setInertia(gtsam::Matrix3::Zero());
     }
   }
 
-  double sigma_dynamics = 1e-6;    // std of dynamics constraints.
-  double sigma_objectives = 1e-7;  // std of additional objectives.
+  double sigma_dynamics = 1e-3;    // std of dynamics constraints.
+  double sigma_objectives = 1e-3;  // std of additional objectives.
 
   // Noise models.
   auto dynamics_model_6 = Isotropic::Sigma(6, sigma_dynamics),
@@ -88,7 +231,8 @@ int main(int argc, char** argv) {
   gtsam::Vector3 gravity(0, 0, -9.8);
   double mu = 1.0;
 
-  OptimizerSetting opt(sigma_dynamics);
+  //OptimizerSetting opt(sigma_dynamics);
+   OptimizerSetting opt(1e-3, 1e-3, 1e-3, 1e-2);
   DynamicsGraph graph_builder(opt, gravity);
 
   // Create the trajectory, 1 walk cycle.
@@ -101,9 +245,9 @@ int main(int argc, char** argv) {
 
   // Build the objective factors.
   double ground_height = 1.0;
-  const Point3 step(0.5, 0, 0);
+  const Point3 step(0.25, 0, 0);
   gtsam::NonlinearFactorGraph objectives =
-      trajectory.contactPointObjectives(robot, Isotropic::Sigma(3, 1e-7), step, ground_height);
+      trajectory.contactPointObjectives(robot, Isotropic::Sigma(3, 1e-6), step, ground_height);
 
   // Get final time step.
   int K = trajectory.getEndTimeStep(trajectory.numPhases() - 1);
@@ -113,8 +257,8 @@ int main(int argc, char** argv) {
   for (int k = 0; k <= K; k++) {
     objectives.add(
         LinkObjectives(base_link->id(), k)
-            .pose(Pose3(Rot3(), Point3(0, 0, 0.4)), Isotropic::Sigma(6, 1e-6))
-            .twist(gtsam::Z_6x1, Isotropic::Sigma(6, 5e-5)));
+            .pose(Pose3(Rot3(), Point3(0, 0, 0.4)), Isotropic::Sigma(6, 1e-2))
+            .twist(gtsam::Z_6x1, Isotropic::Sigma(6, 5e-2)));
   }
 
   // Add link and joint boundary conditions to FG.
@@ -123,41 +267,66 @@ int main(int argc, char** argv) {
                                    objectives_model_1, objectives_model_1);
 
   // Constrain all Phase keys to have duration of 1 /240.
-  const double desired_dt = 1. / 240;
+  const double desired_dt = 1. / 20;
   trajectory.addIntegrationTimeFactors(&objectives, desired_dt, 1e-30);
 
   // Add min torque objectives.
-  trajectory.addMinimumTorqueFactors(&objectives, robot, Unit::Create(1));
+  //trajectory.addMinimumTorqueFactors(&objectives, robot, Unit::Create(1));
 
   // Add prior on A1 lower joint angles
-  auto prior_model = Isotropic::Sigma(1, 1e-6);
+  auto prior_model_angles = Isotropic::Sigma(1, 1e-2);
+  auto prior_model_hip = Isotropic::Sigma(1, 1e-2);
   for (auto&& joint : robot.joints())
     if (joint->name().find("lower") < 100)
       for (int k = 0; k <= K; k++) {
         //std::cout << joint->name() << joint->name().find("lower") << std::endl;
-        objectives.add(JointObjectives(joint->id(), k).angle(-1.4, prior_model));
+        objectives.add(JointObjectives(joint->id(), k).angle(-1.4, prior_model_angles));
       }
-  // Add objectives to factor graph.
-  graph.add(objectives);
+
+  // Add prior on A1 lower joint angles
+  /*for (auto&& joint : robot.joints())
+    if (joint->name().find("upper") < 100)
+      for (int k = 0; k <= K; k++) {
+        //std::cout << joint->name() << joint->name().find("lower") << std::endl;
+        objectives.add(JointObjectives(joint->id(), k).torque(0.0, prior_model_torques));
+      }
+
+    // Add prior on A1 lower joint angles
+  for (auto&& joint : robot.joints())
+    if (joint->name().find("hip") < 100)
+      for (int k = 0; k <= K; k++) {
+        //std::cout << joint->name() << joint->name().find("lower") << std::endl;
+        objectives.add(JointObjectives(joint->id(), k).torque(0.0, prior_model_torques));
+      } */
 
   // Add prior on A1 hip joint angles
-  prior_model = Isotropic::Sigma(1, 1e-7);
+  //prior_model = Isotropic::Sigma(1, 1e-3);
   for (auto&& joint : robot.joints())
     if (joint->name().find("hip") < 100)
       for (int k = 0; k <= K; k++) {
         //std::cout << joint->name() << joint->name().find("hip") << std::endl;
-        objectives.add(JointObjectives(joint->id(), k).angle(0.0, prior_model));
+        objectives.add(JointObjectives(joint->id(), k).angle(0.0, prior_model_hip));
+      }
+ 
+   // Add prior on A1 hip joint angles
+  //prior_model = Isotropic::Sigma(1, 1e-3);
+  for (auto&& joint : robot.joints())
+    if (joint->name().find("upper") < 100)
+      for (int k = 0; k <= K; k++) {
+        //std::cout << joint->name() << joint->name().find("hip") << std::endl;
+        objectives.add(JointObjectives(joint->id(), k).angle(0.7, prior_model_angles));
       }
   // Add objectives to factor graph.
    graph.add(objectives);
- 
+
   // Initialize solution.
-  double gaussian_noise = 1e-3;
+  double gaussian_noise = 1e-30;
  Initializer initializer;
   gtsam::Values init_vals =
       trajectory.multiPhaseInitialValues(robot, initializer, gaussian_noise, desired_dt);
 
-  gttic_(optimization);
+
+  gttic_(optimization2);
 
   std::cout << "graph size = " << graph.size() << std::endl;
   std::cout << "graph keys size = " << graph.keys().size() << std::endl;
@@ -173,12 +342,184 @@ int main(int argc, char** argv) {
   gtsam::LevenbergMarquardtOptimizer optimizer(graph, init_vals, params);
   auto results = optimizer.optimize();
 
-  gttoc_(optimization);
-  gttoc_(start);
+  gttoc_(optimization2);
+  gttoc_(start2);
+
+  //gtsam::tictoc_print_(); //need to build gtsam with timing for this
+  // Write results to traj file
+  trajectory.writeToFile(robot, "a1_traj_DG_massless.csv", results);
+
+  return 0;
+}
+
+int newGraph(){
+
+  gttic_(start3); //need to build gtsam with timing for this
+
+  // Load Unitree A1 robot urdf.
+  auto robot =
+      CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1");
+
+
+  // set masses and inertias
+  for (auto&& link : robot.links()) {
+    if (link->name().find("trunk") == std::string::npos) {
+      link->setMass(0.0);
+      link->setInertia(gtsam::Matrix3::Zero());
+    }
+  }
+
+  double sigma_dynamics = 1e-3;    // std of dynamics constraints.
+  double sigma_objectives = 1e-3;  // std of additional objectives.
+
+  // Noise models.
+  auto dynamics_model_6 = Isotropic::Sigma(6, sigma_dynamics),
+       dynamics_model_1 = Isotropic::Sigma(1, sigma_dynamics),
+       objectives_model_6 = Isotropic::Sigma(6, sigma_objectives),
+       objectives_model_1 = Isotropic::Sigma(1, sigma_objectives);
+
+  // Env parameters.
+  gtsam::Vector3 gravity(0, 0, -9.8);
+  double mu = 1.0;
+
+  //OptimizerSetting opt(sigma_dynamics);
+  OptimizerSetting opt;
+  ChainDynamicsGraph graph_builder(robot, opt, gravity);
+
+  // Create the trajectory, 1 walk cycle.
+  auto trajectory = getTrajectory(robot, 1);
+
+  // Create multi-phase trajectory factor graph
+  auto collocation = CollocationScheme::Euler;
+  auto graph =
+      trajectory.multiPhaseFactorGraph(robot, graph_builder, collocation, mu);
+  
+  // Build the objective factors.
+  double ground_height = 1.0;
+  const Point3 step(0.25, 0, 0);
+  gtsam::NonlinearFactorGraph objectives =
+      trajectory.contactPointObjectives(robot, Isotropic::Sigma(3, 1e-6), step, ground_height);
+
+  // Get final time step.
+  int K = trajectory.getEndTimeStep(trajectory.numPhases() - 1);
+
+  // boundary conditions, using this instead addBoundaryConditions because we dont have link twist and accel variables
+  for (auto &&link : robot.links()) {
+    // Initial link poses should be bMcom
+    const int i = link->id();
+    if (i==0)
+      objectives.add(LinkObjectives(i, 0)
+                   .pose(link->bMcom(), Isotropic::Sigma(6, 1e-3))
+                   .twist(gtsam::Z_6x1, Isotropic::Sigma(6, 1e-3)));
+    if (i==3 || i==6 || i==9 || i==12)
+      objectives.add(LinkObjectives(i, 0)
+                   .pose(link->bMcom(), Isotropic::Sigma(6, 1e-3)));
+    
+  }
+  // initial and end joint velocity and accelerations should be zero
+  objectives.add(JointsAtRestObjectives(robot, objectives_model_1,
+                                  objectives_model_1, 0));
+  objectives.add(JointsAtRestObjectives(robot, objectives_model_1,
+                                  objectives_model_1, K));
+
+  // Add body goal objectives to the factor graph.
+  auto base_link = robot.link("trunk");
+  for (int k = 0; k <= K; k++) {
+   objectives.add(
+        LinkObjectives(base_link->id(), k)
+            .pose(Pose3(Rot3(), Point3(0, 0, 0.4)), Isotropic::Sigma(6, 1e-2))
+            .twist(gtsam::Z_6x1, Isotropic::Sigma(6, 5e-2)));
+  }
+
+  // Constrain all Phase keys to have duration of 1 /240.
+  const double desired_dt = 1. / 20;
+  trajectory.addIntegrationTimeFactors(&objectives, desired_dt, 1e-30);
+
+  // Add min torque objectives.
+  //trajectory.addMinimumTorqueFactors(&objectives, robot, Unit::Create(1));
+
+  // Add prior on A1 lower joint angles
+  auto prior_model_angles = Isotropic::Sigma(1, 1e-2);
+  auto prior_model_hip = Isotropic::Sigma(1, 1e-2);
+  //auto prior_model_torques = Isotropic::Sigma(1, 1e-1);
+  for (auto&& joint : robot.joints())
+    if (joint->name().find("lower") < 100)
+      for (int k = 0; k <= K; k++) {
+        //std::cout << joint->name() << joint->name().find("lower") << std::endl;
+        objectives.add(JointObjectives(joint->id(), k).angle(-1.4, prior_model_angles));
+      }
+
+    // Add prior on A1 lower joint angles
+  //auto prior_model = Isotropic::Sigma(1, 1e-3);
+  /* for (auto&& joint : robot.joints())
+    if (joint->name().find("upper") < 100)
+      for (int k = 0; k <= K; k++) {
+        //std::cout << joint->name() << joint->name().find("lower") << std::endl;
+        objectives.add(JointObjectives(joint->id(), k).torque(0.0, prior_model_torques));
+      }
+
+      // Add prior on A1 lower joint angles
+  for (auto&& joint : robot.joints())
+    if (joint->name().find("hip") < 100)
+      for (int k = 0; k <= K; k++) {
+        //std::cout << joint->name() << joint->name().find("lower") << std::endl;
+        objectives.add(JointObjectives(joint->id(), k).torque(0.0, prior_model_torques));
+      } */
+
+  // Add prior on A1 hip joint angles
+  //prior_model = Isotropic::Sigma(1, 1e-3);
+  for (auto&& joint : robot.joints())
+    if (joint->name().find("hip") < 100)
+      for (int k = 0; k <= K; k++) {
+        //std::cout << joint->name() << joint->name().find("hip") << std::endl;
+        objectives.add(JointObjectives(joint->id(), k).angle(0.0, prior_model_hip));
+      }
+
+     // Add prior on A1 hip joint angles
+  for (auto&& joint : robot.joints())
+    if (joint->name().find("upper") < 100)
+      for (int k = 0; k <= K; k++) {
+        //std::cout << joint->name() << joint->name().find("hip") << std::endl;
+        objectives.add(JointObjectives(joint->id(), k).angle(0.7, prior_model_angles));
+      }
+  // Add objectives to factor graph.
+   graph.add(objectives);
+   
+  // Initialize solution.
+  double gaussian_noise = 1e-30;
+  ChainInitializer initializer;
+  gtsam::Values init_vals =
+      trajectory.multiPhaseInitialValues(robot, initializer, gaussian_noise, desired_dt);
+
+  gttic_(optimization3);
+
+  std::cout << "graph size = " << graph.size() << std::endl;
+  std::cout << "graph keys size = " << graph.keys().size() << std::endl;
+  std::cout << "init vals size = " << init_vals.size() << std::endl;
+
+  // Optimize!
+  gtsam::LevenbergMarquardtParams params;
+  //params.setVerbosityLM("SUMMARY");
+  params.setlambdaInitial(1e10);
+  params.setlambdaLowerBound(1e-7);
+  params.setlambdaUpperBound(1e10);
+  params.setAbsoluteErrorTol(1.0);
+  gtsam::LevenbergMarquardtOptimizer optimizer(graph, init_vals, params);
+  auto results = optimizer.optimize();
+
+  gttoc_(optimization3);
+  gttoc_(start3);
 
   gtsam::tictoc_print_(); //need to build gtsam with timing for this
   // Write results to traj file
-  trajectory.writeToFile(robot, "a1_traj.csv", results);
+  trajectory.writeToFile(robot, "a1_traj_CDG_massless.csv", results);
 
   return 0;
 }
+
+int main(int argc, char** argv) {
+  int a = massGraph();
+  int b = oldGraph();
+  int c = newGraph();
+  return 0;
+}
\ No newline at end of file
diff --git a/examples/example_a1_walking/main_combined.cpp b/examples/example_a1_walking/main_combined.cpp
new file mode 100644
index 00000000..f6e5dc75
--- /dev/null
+++ b/examples/example_a1_walking/main_combined.cpp
@@ -0,0 +1,373 @@
+/* ----------------------------------------------------------------------------
+ * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file  main_combined.cpp
+ * @brief Unitree A1 trajectory optimization with pre-specified footholds.
+ * @author: Dan Barladeanu
+ */
+
+#include <gtdynamics/factors/ObjectiveFactors.h>
+#include <gtdynamics/universal_robot/sdf.h>
+#include <gtdynamics/utils/Trajectory.h>
+#include <gtdynamics/dynamics/ChainDynamicsGraph.h>
+#include <gtdynamics/utils/ChainInitializer.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/base/timing.h>
+
+#include <algorithm>
+#include <fstream>
+#include <iostream>
+#include <utility>
+
+using std::string;
+using std::vector;
+
+using gtsam::Point3;
+using gtsam::Pose3;
+using gtsam::Rot3;
+using gtsam::Vector6;
+using gtsam::noiseModel::Isotropic;
+using gtsam::noiseModel::Unit;
+
+using namespace gtdynamics;
+
+// Returns a Trajectory object for a single a1 walk cycle.
+Trajectory getTrajectory(const Robot& robot, size_t repeat) {
+  vector<LinkSharedPtr> rlfr = {robot.link("RL_lower"),robot.link("FR_lower")};
+  vector<LinkSharedPtr> rrfl = {robot.link("RR_lower"),robot.link("FL_lower")};
+  auto all_feet = rlfr;
+  all_feet.insert(all_feet.end(), rrfl.begin(), rrfl.end());
+
+  // Create three different FootContactConstraintSpecs, one for all the feet on the
+  // ground, one with RL and FR, one with RR and FL
+  const Point3 contact_in_com(0, 0, -0.07);
+  auto stationary = std::make_shared<FootContactConstraintSpec>(all_feet, contact_in_com);
+  auto RLFR = std::make_shared<FootContactConstraintSpec>(rlfr, contact_in_com);
+  auto RRFL = std::make_shared<FootContactConstraintSpec>(rrfl, contact_in_com);
+
+  //FootContactVector states = {noRL, noRR, noFR, noFL};
+  FootContactVector states = {stationary, RLFR, stationary, RRFL};
+  std::vector<size_t> phase_lengths = {2, 10, 2, 10};
+
+  WalkCycle walk_cycle(states, phase_lengths);
+
+  return Trajectory(walk_cycle, repeat);
+}
+
+int CombinedRun(bool add_mass_to_body){
+
+
+  //gttic_(start1); //need to build gtsam with timing for this
+
+  // Load Unitree A1 robot urdf.
+  auto robot =
+      CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1");
+
+    // Load Unitree A1 robot urdf.
+  auto robot_massless =
+      CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1");
+
+    // set masses and inertias
+  double total_mass = 0.0;
+  for (auto&& link : robot_massless.links()) {
+    if (link->name().find("trunk") == std::string::npos) {
+      link->setMass(0.0);
+      link->setInertia(gtsam::Matrix3::Zero());
+    }
+  }
+  
+  if (add_mass_to_body) {
+    robot_massless.link("trunk")->setMass(12.458);
+    std::cout << "robot body mass set to " << robot_massless.link("trunk")->mass() << std::endl;
+  }
+
+  double sigma_dynamics =5e-5;    // std of dynamics constraints.
+  double sigma_objectives = 1e-3;  // std of additional objectives.
+
+  // Noise models.
+  auto dynamics_model_6 = Isotropic::Sigma(6, sigma_dynamics),
+       dynamics_model_1 = Isotropic::Sigma(1, sigma_dynamics),
+       objectives_model_6 = Isotropic::Sigma(6, sigma_objectives),
+       objectives_model_1 = Isotropic::Sigma(1, sigma_objectives);
+
+  // Env parameters.
+  gtsam::Vector3 gravity(0, 0, -9.8);
+  double mu = 1.0;
+
+  OptimizerSetting opt(sigma_dynamics, sigma_dynamics, sigma_dynamics, sigma_dynamics);
+  opt.p_cost_model = gtsam::noiseModel::Constrained::All(6);
+  //opt.q_col_cost_model = Isotropic::Sigma(1, 1e-2);
+  //opt.v_col_cost_model = Isotropic::Sigma(1, 1e-2);
+  //opt.cm_cost_model = gtsam::noiseModel::Constrained::All(3);
+  //opt.f_cost_model = gtsam::noiseModel::Constrained::All(6);
+  //opt.fa_cost_model = gtsam::noiseModel::Constrained::All(6);
+  //opt.t_cost_model = gtsam::noiseModel::Constrained::All(1);
+  //OptimizerSetting opt(1e-3, 1e-3, 1e-3, 1e-2);
+  DynamicsGraph graph_mass_builder(opt, gravity);
+  DynamicsGraph graph_massless_builder(opt,gravity);
+  ChainDynamicsGraph chain_graph_massless_builder(robot_massless, opt, gravity);
+
+  // Create the trajectory, 1 walk cycle.
+  auto trajectory = getTrajectory(robot, 3);
+
+  // Create multi-phase trajectory factor graph
+  auto collocation = CollocationScheme::Euler;
+  std::cout<<"graph_DG_mass"<<std::endl;
+  auto graph_DG_mass =
+      trajectory.multiPhaseFactorGraph(robot, graph_mass_builder, collocation, mu);
+  std::cout<<"graph_DG_massless"<<std::endl;
+  auto graph_DG_massless =
+      trajectory.multiPhaseFactorGraph(robot_massless, graph_massless_builder, collocation, mu);
+  std::cout<<"graph_CDG_massless"<<std::endl;
+  auto graph_CDG_massless =
+      trajectory.multiPhaseFactorGraph(robot_massless, chain_graph_massless_builder, collocation, mu);
+
+  // Build the objective factors.
+  double ground_height = -0.4;
+  const Point3 step(0.18, 0.0, 0);
+  gtsam::NonlinearFactorGraph objectives =
+      trajectory.contactPointObjectives(robot, Isotropic::Sigma(3,8e-6), step, ground_height);
+
+    // Get final time step.
+  int K = trajectory.getEndTimeStep(trajectory.numPhases() - 1);
+
+  // Add body goal objectives to the factor graph.
+  auto base_link = robot.link("trunk");
+  std::cout << "base link mass = " << base_link->mass() << std::endl; 
+  for (int k = 0; k <= K; k++) {
+    objectives.add(
+        LinkObjectives(base_link->id(), k)
+            .pose(base_link->bMcom(), Isotropic::Sigma(6, 5e-5))
+            .twist(gtsam::Z_6x1, Isotropic::Sigma(6, 5e-5))
+            .twistAccel(gtsam::Z_6x1, Isotropic::Sigma(6, 5e-5)));
+  }
+
+  // Add prior on A1 lower joint angles
+  auto prior_model_lower= Isotropic::Sigma(1, 5e-5);
+  auto prior_model_upper = Isotropic::Sigma(1, 5e-5);
+  auto prior_model_hip = Isotropic::Sigma(1, 5e-5);
+  auto prior_model_va = Isotropic::Sigma(1, 1e-1);
+  for (auto&& joint : robot.joints())
+    if (joint->name().find("lower") < 100)
+      for (int k = 0; k <= K; k++) {
+        //std::cout << joint->name() << joint->name().find("lower") << std::endl;
+        objectives.add(JointObjectives(joint->id(), k).angle(-1.4, prior_model_lower));
+        //.velocity(0.0, prior_model_va).acceleration(0.0, prior_model_va));
+      }
+
+  // Add prior on A1 hip joint angles
+  //prior_model = Isotropic::Sigma(1, 1e-3);
+  for (auto&& joint : robot.joints())
+    if (joint->name().find("hip") < 100)
+      for (int k = 0; k <= K; k++) {
+        //std::cout << joint->name() << joint->name().find("hip") << std::endl;
+        objectives.add(JointObjectives(joint->id(), k).angle(0.0, prior_model_hip));
+        //.velocity(0.0, prior_model_va).acceleration(0.0, prior_model_va));
+      }
+ 
+   // Add prior on A1 hip joint angles
+  //prior_model = Isotropic::Sigma(1, 1e-3);
+  for (auto&& joint : robot.joints())
+    if (joint->name().find("upper") < 100)
+      for (int k = 0; k <= K; k++) {
+        //std::cout << joint->name() << joint->name().find("hip") << std::endl;
+        objectives.add(JointObjectives(joint->id(), k).angle(0.9, prior_model_upper));
+        //.velocity(0.0, prior_model_va).acceleration(0.0, prior_model_va));
+      }
+
+    // Constrain all Phase keys to have duration of 1 /20.
+  const double desired_dt = 1. / 24;
+  trajectory.addIntegrationTimeFactors(&objectives, desired_dt, 1e-30);
+
+  // Add objectives to factor graph.
+   graph_DG_mass.add(objectives);
+   graph_DG_massless.add(objectives);
+   graph_CDG_massless.add(objectives);
+
+  gtsam::NonlinearFactorGraph boundary_objectives_DG;
+  gtsam::NonlinearFactorGraph boundary_objectives_CDG;
+
+  // Add link and joint boundary conditions to FG.
+  trajectory.addBoundaryConditions(&boundary_objectives_DG, robot, dynamics_model_6,
+                                   dynamics_model_6, objectives_model_6,
+                                  objectives_model_1, objectives_model_1);
+
+  // boundary conditions, using this instead addBoundaryConditions because we dont have link twist and accel variables
+  for (auto &&link : robot_massless.links()) {
+    // Initial link poses should be bMcom
+    const int i = link->id();
+    if (i==0)
+      boundary_objectives_CDG.add(LinkObjectives(i, 0)
+                   .pose(link->bMcom(), dynamics_model_6)
+                   .twist(gtsam::Z_6x1, dynamics_model_6));
+    if (i==3 || i==6 || i==9 || i==12)
+      boundary_objectives_CDG.add(LinkObjectives(i, 0)
+                   .pose(link->bMcom(), dynamics_model_6));
+    
+  }
+  // initial and end joint velocity and accelerations should be zero
+  boundary_objectives_CDG.add(JointsAtRestObjectives(robot, objectives_model_1,
+                                  objectives_model_1, 0));
+  boundary_objectives_CDG.add(JointsAtRestObjectives(robot, objectives_model_1,
+                                  objectives_model_1, K));
+
+    // Add objectives to factor graph.
+   graph_DG_mass.add(boundary_objectives_DG);
+   graph_DG_massless.add(boundary_objectives_DG);
+   graph_CDG_massless.add(boundary_objectives_CDG);
+
+  // Initialize solution.
+  double gaussian_noise = 1e-20;
+ Initializer initializer;
+ ChainInitializer chain_initializer;
+  gtsam::Values init_vals_DG =
+      trajectory.multiPhaseInitialValues(robot, initializer, gaussian_noise, desired_dt);
+  gtsam::Values init_vals_CDG =
+      trajectory.multiPhaseInitialValues(robot_massless, chain_initializer, gaussian_noise, desired_dt);
+
+    // Optimize!
+  gtsam::LevenbergMarquardtParams params;
+  //params.setVerbosityLM("SUMMARY");
+  params.setlambdaInitial(1e10);
+  params.setlambdaLowerBound(1e-7);
+  params.setlambdaUpperBound(1e10);
+  params.setAbsoluteErrorTol(1.0);
+
+  gttic_(optimization1);
+
+  std::cout << "graph_DG_mass size = " << graph_DG_mass.size() << std::endl;
+  std::cout << "graph_DG_mass keys size = " << graph_DG_mass.keys().size() << std::endl;
+  std::cout << "graph_DG_mass init vals size = " << init_vals_DG.size() << std::endl;
+  
+  // Optimize!
+  gtsam::LevenbergMarquardtOptimizer optimizer_DG_mass(graph_DG_mass, init_vals_DG, params);
+  auto results_DG_mass = optimizer_DG_mass.optimize();
+
+  gttoc_(optimization1);
+  //gttoc_(start1);
+
+  //gtsam::tictoc_print_(); //need to build gtsam with timing for this
+  // Write results to traj file
+  trajectory.writeToFile(robot, "a1_traj_DG_mass.csv", results_DG_mass);
+
+ gttic_(optimization2);
+
+  std::cout << "graph_DG_massless size = " << graph_DG_massless.size() << std::endl;
+  std::cout << "graph_DG_massless keys size = " << graph_DG_massless.keys().size() << std::endl;
+  std::cout << "graph_DG_massless init vals size = " << init_vals_DG.size() << std::endl;
+  
+  // Optimize!
+  gtsam::LevenbergMarquardtOptimizer optimizer_DG_massless(graph_DG_massless, init_vals_DG, params);
+  auto results_DG_massless = optimizer_DG_massless.optimize();
+
+  gttoc_(optimization2);
+
+  //gtsam::tictoc_print_(); //need to build gtsam with timing for this
+  // Write results to traj file
+  trajectory.writeToFile(robot_massless, "a1_traj_DG_massless.csv", results_DG_massless);
+  
+   gttic_(optimization3);
+
+  std::cout << "graph_CDG_massless size = " << graph_CDG_massless.size() << std::endl;
+  std::cout << "graph_CDG_massless keys size = " << graph_CDG_massless.keys().size() << std::endl;
+  std::cout << "graph_CDG_massless init vals size = " << init_vals_CDG.size() << std::endl;
+  
+  // Optimize!
+  gtsam::LevenbergMarquardtOptimizer optimizer_CDG_massless(graph_CDG_massless, init_vals_CDG, params);
+  auto results_CDG_massless = optimizer_CDG_massless.optimize();
+
+  gttoc_(optimization3);
+
+  //gtsam::tictoc_print_(); //need to build gtsam with timing for this
+  // Write results to traj file
+  trajectory.writeToFile(robot_massless, "a1_traj_CDG_massless.csv", results_CDG_massless);
+
+  gtsam::tictoc_print_(); 
+
+  std::ofstream FileLocations("locations.txt");
+  std::ofstream FileTorques;
+  std::ofstream FileWrenchesBody;
+  std::ofstream FileWrenchesGround;
+  if (add_mass_to_body){
+    FileWrenchesBody.open("body_wrenches_body_full_mass.txt");
+    FileWrenchesGround.open("ground_wrenches_body_full_mass.txt");
+    FileTorques.open("torques_body_full_mass.txt");
+  }
+  else {
+    FileWrenchesBody.open("body_wrenches_body_only_mass.txt");
+    FileWrenchesGround.open("ground_wrenches_body_only_mass.txt");
+    FileTorques.open("torques_body_only_mass.txt");
+  }
+
+  const int link_to_test = 0;
+  const int joint_to_test = 0;
+  for (int i = 0; i < K;++i){
+    //std::cout << results_DG_massless.at<gtsam::Vector6>(WrenchKey(0,0,i)) << std::endl;
+    //std::cout << results_CDG_massless.at<gtsam::Vector6>(WrenchKey(0,0,i)) << std::endl;
+    gtsam::Pose3 pose1 = results_DG_mass.at<gtsam::Pose3>(PoseKey(link_to_test,i));
+    gtsam::Pose3 pose2 =  results_DG_massless.at<gtsam::Pose3>(PoseKey(link_to_test,i));
+    gtsam::Pose3 pose3 = results_CDG_massless.at<gtsam::Pose3>(PoseKey(link_to_test,i)) ;
+    FileLocations << pose1.translation().x() << "," << pose1.translation().y() <<","<< pose1.translation().z() <<","<<
+                        pose2.translation().x() << "," << pose2.translation().y() << ","<<pose2.translation().z() <<","<<
+                        pose3.translation().x() << "," << pose3.translation().y() << ","<<pose3.translation().z() <<"\n";
+    gtsam::Vector6 wrench1 = results_DG_mass.at<gtsam::Vector6>(WrenchKey(0,joint_to_test,i));
+    gtsam::Vector6 wrench2 = results_DG_massless.at<gtsam::Vector6>(WrenchKey(0,joint_to_test,i));
+    gtsam::Vector6 wrench3= results_CDG_massless.at<gtsam::Vector6>(WrenchKey(0,joint_to_test,i));
+    FileWrenchesBody << wrench1[0] << "," << wrench1[1]  <<","<< wrench1[2]  <<","<< wrench1[3]<<","<< wrench1[4]<<","<< wrench1[5]<<","<<
+                                      wrench2[0] << "," << wrench2[1]  <<","<< wrench2[2]  <<","<< wrench2[3]<<","<< wrench2[4]<<","<< wrench2[5]<<","<<
+                                       wrench3[0] << "," << wrench3[1]  <<","<< wrench3[2]  <<","<< wrench3[3]<<","<< wrench3[4]<<","<< wrench3[5]<<"\n";
+
+    double angle0 = results_CDG_massless.at<double>(JointAngleKey(0,i));
+    double angle1 = results_CDG_massless.at<double>(JointAngleKey(1,i));
+    double angle2 = results_CDG_massless.at<double>(JointAngleKey(2,i));
+    Pose3 T0 = robot.joint("FL_hip_joint")->parentTchild(angle0);
+    Pose3 T1 = robot.joint("FL_upper_joint")->parentTchild(angle1);
+    Pose3 T2 = robot.joint("FL_lower_joint")->parentTchild(angle2);
+    Pose3 T3 = Pose3(Rot3(),Point3(0.0, 0.0, -0.07));
+    Pose3 T_body_contact = T0 * T1 * T2 * T3;
+
+    double torque0_dg_mass = results_DG_mass.at<double>(TorqueKey(0,i));
+    double torque1_dg_mass = results_DG_mass.at<double>(TorqueKey(1,i));
+    double torque2_dg_mass = results_DG_mass.at<double>(TorqueKey(2,i));
+    
+    double torque0_dg_massless = results_DG_mass.at<double>(TorqueKey(0,i));
+    double torque1_dg_massless = results_DG_mass.at<double>(TorqueKey(1,i));
+    double torque2_dg_massless = results_DG_mass.at<double>(TorqueKey(2,i));
+
+    gtsam::Vector6 wrench6= results_CDG_massless.at<gtsam::Vector6>(WrenchKey(0,0,i));
+
+    double torque0_cdg_massless = (-1) * wrench6.transpose() * T0.AdjointMap() * robot.joint("FL_hip_joint")->cScrewAxis();
+    double torque1_cdg_massless = (-1) * wrench6.transpose() * (T0*T1).AdjointMap()* robot.joint("FL_upper_joint")->cScrewAxis();
+    double torque2_cdg_massless = (-1) * wrench6.transpose() * (T0*T1*T2).AdjointMap()* robot.joint("FL_lower_joint")->cScrewAxis();
+    
+    FileTorques<<  torque0_dg_mass <<',' << torque1_dg_mass << ',' << torque2_dg_mass << ','<<
+                                    torque0_dg_massless <<',' << torque1_dg_massless << ',' << torque2_dg_massless << ','<<
+                                    torque0_cdg_massless <<',' << torque1_cdg_massless << ',' << torque2_cdg_massless << '\n';
+
+    
+    if (i <13 || i >25) continue;
+
+    gtsam::Vector6 wrench4 = results_DG_mass.at<gtsam::Vector6>(ContactWrenchKey(3,0,i)).transpose() * T3.AdjointMap();
+    gtsam::Vector6 wrench5 = results_DG_massless.at<gtsam::Vector6>(ContactWrenchKey(3,0,i)).transpose() * T3.AdjointMap();
+    gtsam::Vector6 wrench7 = wrench6.transpose() * T_body_contact.AdjointMap();
+    FileWrenchesGround << wrench4[0] << "," << wrench4[1]  <<","<<wrench4[2]  <<","<< wrench4[3]<<","<< wrench4[4]<<","<< wrench4[5]<<","<<
+                                      wrench5[0] << "," << wrench5[1]  <<","<<wrench5[2]  <<","<< wrench5[3]<<","<< wrench5[4]<<","<< wrench5[5]<<","<<
+                                       wrench7[0] << "," << wrench7[1]  <<","<< wrench7[2]  <<","<< wrench7[3]<<","<< wrench7[4]<<","<< wrench7[5]<<"\n";
+  }
+  FileLocations.close();
+  FileWrenchesBody.close();
+  FileWrenchesGround.close();
+  return 0;
+}
+
+int main(int argc, char** argv) {
+  int a = CombinedRun(false);
+  int b = CombinedRun(true);
+  return 0;
+}
\ No newline at end of file
diff --git a/examples/example_a1_walking/sim.py b/examples/example_a1_walking/sim.py
index 896165d7..df1cbbad 100644
--- a/examples/example_a1_walking/sim.py
+++ b/examples/example_a1_walking/sim.py
@@ -12,11 +12,12 @@
 
 _ = p.connect(p.GUI)
 p.setAdditionalSearchPath(pybullet_data.getDataPath())  # To load plane SDF.
+print(pybullet_data.getDataPath())
 p.setGravity(0, 0, -9.8)
-p.setRealTimeSimulation(0)
+p.setRealTimeSimulation(1)
 planeId = p.loadURDF("plane.urdf")
-p.changeDynamics(planeId, -1, lateralFriction=1)
-robot = p.loadURDF(gtd.URDF_PATH + "/a1/a1.urdf",[0,0,0.48],[0,0,0,1])
+p.changeDynamics(planeId, -1, lateralFriction=0.4)
+robot = p.loadURDF(gtd.URDF_PATH + "/a1/a1.urdf",[0,0,0.45],[0,0,0,1.0])
 
 robot_id = robot
 
@@ -28,29 +29,41 @@
         joint_to_jid_map[jinfo[1].decode("utf-8")] = jinfo[0]
 
 #Read walk forward trajectory file from build (after running the example)
-df = pd.read_csv(gtd.URDF_PATH + '/../../build/examples/example_a1_walking/a1_traj.csv')
+df = pd.read_csv(gtd.URDF_PATH + '/../../build/examples/example_a1_walking/a1_traj_DG_mass.csv')
 
 pos, orn = p.getBasePositionAndOrientation(robot_id)
 
 print("Init Base\n\tPos: {}\n\tOrn: {}".format(pos,
                                                p.getEulerFromQuaternion(orn)))
 
+# for i in range(0,20,1):
+    # print(i, p.getJointInfo(robot_id, i))
+    #import pdb;pdb.set_trace()
+    #print(i, p.getLinkInfo(robot_id, i)[1])
+#print(p.getDynamicsInfo(planeId,-1))
 debug_iters = 1
 
-max_traj_replays = 200
+max_traj_replays = 1
 num_traj_replays = 0
 
 t = 0
 t_f = None
 ts = []
 all_pos_sim = []
+all_torques_hip = []
+all_torques_upper = []
+all_torques_lower = []
 
 link_dict = {}
-link_to_num = {10:1, 5: 1, 20: 1, 15: 3}
+link_to_num = {10:1, 5: 4, 20: 10, 15: 7}
 constrained = []
 
 i = 1
 k = 0 
+# for i in range(0,10,1):
+    # print(i)
+    # time.sleep(0.5)
+
 while True:
     if num_traj_replays == max_traj_replays:
         break
@@ -69,7 +82,7 @@
     for joint_id in joint_to_jid_map.values():
         p.setJointMotorControl2(robot_id,joint_id,
                                     controlMode=p.VELOCITY_CONTROL,
-                                    force=500)
+                                    force=50000)
 
 
     for joint, angle in jangles.items():
@@ -78,42 +91,47 @@
                                         jointIndex=joint_to_jid_map[joint],
                                         controlMode=p.POSITION_CONTROL,
                                         targetPosition=angle,
-                                        targetVelocity=target_velocity)
+                                        targetVelocity=0.0)
 
     # Update body CoM coordinate frame.
     new_pos, new_orn = p.getBasePositionAndOrientation(robot_id)
     new_pos = np.array(new_pos)
     new_R = np.array(p.getMatrixFromQuaternion(new_orn)).reshape(3, 3)
-
     # print(i)
+    t_hip = p.getJointState(bodyUniqueId=robot_id, jointIndex = 6)[3]
+    t_upper= p.getJointState(bodyUniqueId=robot_id, jointIndex = 8)[3]
+    t_lower = p.getJointState(bodyUniqueId=robot_id, jointIndex = 9)[3]
 
     # Detect collision points and constrain them.
     '''cp = np.asarray(p.getContactPoints(bodyA=planeId, bodyB=robot_id))
+    #import pdb;pdb.set_trace()
     if cp.shape[0] > 1 and i > 1:
         new_cps = set(cp[:, 4])
 
-        change = list(df.loc[i][np.arange(0, 12)] -
-                      df.loc[i-1][np.arange(0, 12)])
-        # import pdb;pdb.set_trace()
+        change = list(df.loc[i][np.arange(0, 12)] - df.loc[i-1][np.arange(0, 12)])
+        #import pdb;pdb.set_trace()
         # Initial collision
         just_collided = [
             x for x in new_cps if x not in constrained and x in link_to_num.keys()]
         # print(new_cps)
         for x in just_collided:
-            if (link_to_num[x] < 4 and change[link_to_num[x]] >= 0) or (link_to_num[x] >= 4 and change[link_to_num[x]] <= 0):
+            print(change[link_to_num[x]] )
+           # if (link_to_num[x] < 4 and change[link_to_num[x]] >= 0) or (link_to_num[x] >= 4 and change[link_to_num[x]] <= 0):
+            if (change[link_to_num[x]] >= 0.01) or (change[link_to_num[x]] <= -0.01):
                 link_dict[x] = p.createConstraint(robot_id, x, planeId, -1, p.JOINT_POINT2POINT, [
                                                   0, 0, 0], [0, 0, 0], p.getLinkState(robot_id, x)[0])
                 constrained.append(x)
-        print(link_dict)
+        #print(link_dict)
         # Wants to lift
         for x in constrained:
-            if (link_to_num[x] < 4 and change[link_to_num[x]] <= 0) or (link_to_num[x] >= 4 and change[link_to_num[x]] >= 0) and link_dict.get(x) != None:
+            #if (link_to_num[x] < 4 and change[link_to_num[x]] <= 0) or (link_to_num[x] >= 4 and change[link_to_num[x]] >= 0) and link_dict.get(x) != None:
+            if (change[link_to_num[x]] >= 0.01) or (change[link_to_num[x]] <= -0.01) and link_dict.get(x) != None:
                 numConstraints_before = p.getNumConstraints()
                 p.removeConstraint(link_dict[x])
                 if numConstraints_before != p.getNumConstraints():
                     constrained.remove(x) '''
 
-    '''if (i % debug_iters) == 0:
+    if (i % debug_iters) == 0:
         # print("\tIter {} Base\n\t\tPos: {}\n\t\tOrn: {}".format(
         # i, new_pos, p.getEulerFromQuaternion(new_orn)))
 
@@ -122,10 +140,10 @@
                                1, 0, 1], lineWidth=2.5)
         else:
             p.addUserDebugLine(pos, new_pos, lineColorRGB=[
-                               0, 1, 1], lineWidth=2.5)
+                               0, 1, 1], lineWidth=2.5) 
         pos, orn = new_pos, new_orn
 
-        bod_debug_line_x = p.addUserDebugLine(
+        '''bod_debug_line_x = p.addUserDebugLine(
             np.array([0, 0, 0]) + new_pos,
             np.matmul(new_R, np.array([0.05, 0, 0])) + new_pos,
             lineColorRGB=[1, 0, 1], lineWidth=1, lifeTime=1.5)
@@ -139,37 +157,69 @@
             lineColorRGB=[1, 0, 1], lineWidth=1, lifeTime=1.5)'''
     
     p.stepSimulation()
-    time.sleep(1. / 20.)
+    time.sleep(1. /24.)
 
-    ts.append(t)
-    t += 1. / 240.
-    all_pos_sim.append(new_pos)
+    t += 1. / 24.
     i += 1
     k += 1
-
+    if t >2 and t < 12:
+       all_pos_sim.append(new_pos)
+       #if t_hip > 80 or t_upper > 80 or t_lower > 80: continue
+       ts.append(t-2)
+       all_torques_hip.append(t_hip)
+       all_torques_upper.append(t_upper)
+       all_torques_lower.append(t_lower)
+
+torques = np.zeros((len(all_torques_hip),4))
+torques[:,0] = np.array(ts)
+torques[:,1] = np.array(all_torques_hip)
+torques[:,2] = np.array(all_torques_upper)
+torques[:,3] = np.array(all_torques_lower)
+np.savetxt('/home/dan/Desktop/Projects/GTDynamics/build/examples/example_a1_walking/torques_pyb.txt', torques)
 
 pos, orn = p.getBasePositionAndOrientation(robot_id)
 print("Final Base\n\tPos: {}\n\tOrn: {}".format(pos,
                                                 p.getEulerFromQuaternion(orn)))
 
-fig, axs = plt.subplots(3, 1, sharex=True)
-fig.subplots_adjust(hspace=0)
+fig, axs = plt.subplots(6, 1, sharex=True)
+fig.subplots_adjust(hspace=0.1)
 
 axs[0].plot(ts, [p[0] for p in all_pos_sim])
-axs[0].axvline(x=t_f, color='k', linestyle='--')
+#axs[0].axvline(x=t_f, color='k', linestyle='--')
 axs[0].set_ylabel('x (m.)')
+axs[0].set_title('Robot Body location Vs. Time')
 
 axs[1].plot(ts, [p[1] for p in all_pos_sim])
-axs[1].axvline(x=t_f, color='k', linestyle='--')
+#axs[1].axvline(x=t_f, color='k', linestyle='--')
 axs[1].set_ylabel('y (m.)')
+axs[1].set_ylim((-0.2,0.2))
 
 axs[2].plot(ts, [p[2] for p in all_pos_sim])
-axs[2].axvline(x=t_f, color='k', linestyle='--')
+#axs[2].axvline(x=t_f, color='k', linestyle='--')
 axs[2].set_ylabel('z (m.)')
+axs[2].set_ylim((0.2,0.45))
+
+axs[3].plot(ts, [t for t in all_torques_hip])
+#axs[2].axvline(x=t_f, color='k', linestyle='--')
+axs[3].set_ylabel('tau hip')
+#axs[3].set_ylim((-10,10))
+
+axs[4].plot(ts, [t for t in all_torques_upper])
+#axs[2].axvline(x=t_f, color='k', linestyle='--')
+axs[4].set_ylabel('tau upper')
+#axs[3].set_ylim((-10,10))
+
+axs[5].plot(ts, [t for t in all_torques_lower])
+#axs[2].axvline(x=t_f, color='k', linestyle='--')
+axs[5].set_ylabel('tau lower')
+#axs[3].set_ylim((-10,10))
 
 plt.xlabel("time (s.)")
 
-#plt.show()
+plt.show()
+#plt.rc('pgf', texsystem='pdflatex')
+#plt.savefig('/home/dan/Desktop/my_papers/thesis/create_simulation/pyb_sim.pgf')
+
 # i = 0
 # while True:
     # i += 1
diff --git a/gtdynamics/dynamics/Chain.cpp b/gtdynamics/dynamics/Chain.cpp
index 7364b84f..d3852ba6 100644
--- a/gtdynamics/dynamics/Chain.cpp
+++ b/gtdynamics/dynamics/Chain.cpp
@@ -13,6 +13,7 @@
 
 #include <gtdynamics/dynamics/Chain.h>
 
+
 namespace gtdynamics {
 
 Chain operator*(const Chain &chainA, const Chain &chainB) {
@@ -46,7 +47,7 @@ Chain Chain::compose(std::vector<Chain> &chains) {
 }
 
 Pose3 Chain::poe(const Vector &q, std::optional<Pose3> fTe,
-                 gtsam::OptionalJacobian<-1, -1> J) {
+                 gtsam::OptionalJacobian<-1, -1> J) const {
   // Check that input has good size
   if (q.size() != length()) {
     throw std::runtime_error(
@@ -119,7 +120,7 @@ gtsam::Vector3 Chain::DynamicalEquality3(
     const gtsam::Vector6 &wrench, const gtsam::Vector3 &angles,
     const gtsam::Vector3 &torques, gtsam::OptionalJacobian<3, 6> H_wrench,
     gtsam::OptionalJacobian<3, 3> H_angles,
-    gtsam::OptionalJacobian<3, 3> H_torques) {
+    gtsam::OptionalJacobian<3, 3> H_torques) const {
   Matrix J;
   poe(angles, {}, J);
   if (H_wrench) {
@@ -167,9 +168,18 @@ gtsam::Vector3 Chain::DynamicalEquality3(
 
 gtsam::Vector3_ Chain::ChainConstraint3(
     const std::vector<JointSharedPtr> &joints, const gtsam::Key wrench_key,
-    size_t k) {
+    size_t k) const {
   // Get Expression for wrench
-  gtsam::Vector6_ wrench(wrench_key);
+  gtsam::Vector6_ wrench_0_T(wrench_key);
+
+  // The constraint is true for the wrench exerted on the end-effector frame, so
+  // we need to adjoint from base to end-effector
+  gtsam::Vector6_ wrench_0_H =
+      (-1) * joints[0]->childAdjointWrench(wrench_0_T, k);
+  gtsam::Vector6_ wrench_1_U =
+      (-1) * joints[1]->childAdjointWrench(wrench_0_H, k);
+  gtsam::Vector6_ wrench_2_L =
+      (-1) * joints[2]->childAdjointWrench(wrench_1_U, k);
 
   // Get expression for joint angles as a column vector of size 3.
   gtsam::Double_ angle0(JointAngleKey(joints[0]->id(), k)),
@@ -189,9 +199,94 @@ gtsam::Vector3_ Chain::ChainConstraint3(
                 std::placeholders::_2, std::placeholders::_3,
                 std::placeholders::_4, std::placeholders::_5,
                 std::placeholders::_6),
-      wrench, angles, torques);
+      wrench_2_L, angles, torques);
 
   return torque_diff;
 }
 
+gtsam::Vector6_ Chain::AdjointWrenchConstraint3(
+    const std::vector<JointSharedPtr> &joints, const gtsam::Key body_wrench_key,
+    size_t k) const {
+  // Get Expression for wrench
+  gtsam::Vector6_ wrench_0_T(body_wrench_key);
+
+  // Get expression for joint angles as a column vector of size 3.
+  gtsam::Double_ angle0(JointAngleKey(joints[0]->id(), k)),
+      angle1(JointAngleKey(joints[1]->id(), k)),
+      angle2(JointAngleKey(joints[2]->id(), k));
+  gtsam::Vector3_ angles(MakeVector3, angle0, angle1, angle2);
+
+  // Get expression of the dynamical equality
+  gtsam::Vector6_ wrench_end_effector(
+      std::bind(&Chain::AdjointWrenchEquality3, this, std::placeholders::_1,
+                std::placeholders::_2, std::placeholders::_3,
+                std::placeholders::_4),
+      angles, wrench_0_T);
+
+  return wrench_end_effector;
+}
+
+gtsam::Vector6 Chain::AdjointWrenchEquality3(
+    const gtsam::Vector3 &angles, const gtsam::Vector6 &wrench_body,
+    gtsam::OptionalJacobian<6, 3> H_angles,
+    gtsam::OptionalJacobian<6, 6> H_wrench_body) const {
+  Matrix J_theta;
+  gtsam::Matrix6 H_T;
+
+  // Get POE transformation from body to end-effector.
+  Pose3 T_theta = poe(angles, {}, H_angles ? &J_theta : nullptr);
+
+  // Get end-effector wrench by Adjoint. This is true for a massless leg.
+  gtsam::Vector6 transformed_wrench =
+      T_theta.AdjointTranspose(wrench_body,H_angles ? &H_T : nullptr , H_wrench_body);
+
+  if (H_angles) {
+    *H_angles =  H_T * J_theta;
+  }
+
+  return transformed_wrench;
+}
+
+gtsam::Vector6_ Chain::Poe3Factor(const std::vector<JointSharedPtr> &joints,
+                                  const gtsam::Key wTb_key,
+                                  const gtsam::Key wTe_key, size_t k) const {
+  // Get Expression for poses
+  gtsam::Pose3_ wTb(wTb_key);
+  gtsam::Pose3_ wTe(wTe_key);
+
+  // Get expression for joint angles as a column vector of size 3.
+  gtsam::Double_ angle0(JointAngleKey(joints[0]->id(), k)),
+      angle1(JointAngleKey(joints[1]->id(), k)),
+      angle2(JointAngleKey(joints[2]->id(), k));
+  gtsam::Vector3_ angles(MakeVector3, angle0, angle1, angle2);
+
+  // Get expression for forward kinematics
+  gtsam::Pose3_ end_effector_pose(
+      std::bind(&Chain::PoeEquality3, this, std::placeholders::_1,
+                std::placeholders::_2),
+      angles);
+
+  // compose 
+  gtsam::Pose3_ wTe_hat = wTb * end_effector_pose;
+
+  // get error expression
+  gtsam::Vector6_ error_expression = gtsam::logmap(wTe_hat, wTe);
+  
+  return error_expression;
+}
+
+gtsam::Pose3 Chain::PoeEquality3(const gtsam::Vector3 &angles,
+                                 gtsam::OptionalJacobian<6, 3> H_angles) const {
+  Matrix J_theta;
+
+  // Get POE transformation from body to end-effector.
+  Pose3 T_theta = poe(angles, {}, H_angles ? &J_theta : nullptr);
+
+  if (H_angles) {
+    *H_angles =J_theta;
+  }
+
+  return T_theta;
+}
+
 }  // namespace gtdynamics
diff --git a/gtdynamics/dynamics/Chain.h b/gtdynamics/dynamics/Chain.h
index a94fd03f..7aaea368 100644
--- a/gtdynamics/dynamics/Chain.h
+++ b/gtdynamics/dynamics/Chain.h
@@ -17,6 +17,7 @@
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/geometry/Pose3.h>
+#include <gtsam/slam/expressions.h>
 
 #include <optional>
 
@@ -96,7 +97,7 @@ class Chain {
    * Exponentials
    */
   Pose3 poe(const Vector &q, std::optional<Pose3> fTe = {},
-            gtsam::OptionalJacobian<-1, -1> J = {});
+            gtsam::OptionalJacobian<-1, -1> J = {}) const;
 
   /**
    * This function implements the dynamic dependency between the
@@ -116,27 +117,86 @@ class Chain {
       const gtsam::Vector3 &torques,
       gtsam::OptionalJacobian<3, 6> H_wrench = {},
       gtsam::OptionalJacobian<3, 3> H_angles = {},
-      gtsam::OptionalJacobian<3, 3> H_torques = {});
+      gtsam::OptionalJacobian<3, 3> H_torques = {}) const;
 
   /**
    * This function creates a gtsam expression of the Chain constraint FOR A
    * 3-LINK CHAIN.
    *
-   * @param joints ............... Vector of joints in the kinematic chain, FROM
-   * END-EFFECTOR TO BODY (first element in the vector is the joint whos child
-   * is the end-effector).
+   * @param joints ............... Vector of joints in the kinematic chain,FROM
+   *  BODY TO END-EFFECTOR.
    * @param wrench_key ........... Key of the wrench applied on the body by the
    * joint closest to the body.
    * @param k .................... Time slice.
    * @return ..................... GTSAM expression of the chain constraint.
    */
   gtsam::Vector3_ ChainConstraint3(const std::vector<JointSharedPtr> &joints,
-                                   const gtsam::Key wrench_key, size_t k);
-};
+                                   const gtsam::Key wrench_key, size_t k) const;
+
+  /**
+   * This function creates a gtsam expression of the End-Effector wrench using
+   * a Chain under massless leg assumption and Product of Exponentials.
+   *
+   * @param joints ............... Vector of joints in the kinematic chain, FROM
+   *  BODY TO END-EFFECTOR.
+   * @param wrench_key ........... Key of the wrench applied on the body by the
+   * joint closest to the body.
+   * @param k .................... Time slice.
+   * @return ..................... GTSAM expression of the chain constraint.
+   */
+  gtsam::Vector6_ AdjointWrenchConstraint3(
+      const std::vector<JointSharedPtr> &joints,
+      const gtsam::Key body_wrench_key, size_t k) const;
+
+  /**
+   *  This function calculates the end-effector wrench using POE and chain.
+   *
+   *
+   *
+   * @param angles .............. angles of the joints in the chain FROM
+   *  BODY TO END-EFFECTOR.
+   * @param wrench_body ......... wrench applied by the first joint in the chain
+   * on the body link
+   * @return ...................  gtsam expression of the  end-effector wrench
+   */
+  gtsam::Vector6 AdjointWrenchEquality3(
+      const gtsam::Vector3 &angles, const gtsam::Vector6 &wrench_body,
+      gtsam::OptionalJacobian<6, 3> H_angles = {},
+      gtsam::OptionalJacobian<6, 6> H_wrench_body = {}) const;
+
+  /**
+   * This function creates a gtsam expression factor of the End-Effector pose
+   * using Product of Exponentials and Chain (forward kinematics).
+   *
+   * @param joints ............... Vector of joints in the kinematic chain, FROM
+   *  BODY TO END-EFFECTOR.
+   * @param wTb_key............... Key of body link pose
+   * @param wTe_key............... Key of end-effector pose
+   * @param cost_model............ cost model for factor
+   * @param k .................... Time slice.
+   * @return ..................... GTSAM expression of the chain constraint.
+   */
+  gtsam::Vector6_ Poe3Factor(const std::vector<JointSharedPtr> &joints,
+                             const gtsam::Key wTb_key, const gtsam::Key wTe_key,
+                             size_t k) const;
+
+  /**
+   *  This function calculates the end-effector pose using POE and chain
+   * (forward kinematics).
+   *
+   * @param angles .............. angles of the joints in the chain FROM
+   *  BODY TO END-EFFECTOR.
+   * @return ...................  gtsam expression of the  end-effector wrench
+   */
+  gtsam::Pose3 PoeEquality3(
+      const gtsam::Vector3 &angles,
+      gtsam::OptionalJacobian<6, 3> H_angles = {}) const;
+
+};  // Chain class
 
 // Helper function to create expression with a vector, used in
 // ChainConstraint3.
-gtsam::Vector3 MakeVector3(const double &value0, const double &value1,
+inline gtsam::Vector3 MakeVector3(const double &value0, const double &value1,
                            const double &value2,
                            gtsam::OptionalJacobian<3, 1> J0 = {},
                            gtsam::OptionalJacobian<3, 1> J1 = {},
diff --git a/gtdynamics/dynamics/ChainDynamicsGraph.cpp b/gtdynamics/dynamics/ChainDynamicsGraph.cpp
new file mode 100644
index 00000000..86977b36
--- /dev/null
+++ b/gtdynamics/dynamics/ChainDynamicsGraph.cpp
@@ -0,0 +1,223 @@
+/* ----------------------------------------------------------------------------
+ * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file DynamicsGraph.h
+ * @brief Builds a lean dynamics graph from a Robot object.
+ * @author Dan Barladeanu
+ */
+
+#include "gtdynamics/dynamics/ChainDynamicsGraph.h"
+
+namespace gtdynamics {
+
+std::vector<std::vector<JointSharedPtr>> ChainDynamicsGraph::getChainJoints(const Robot& robot) {
+  std::vector<JointSharedPtr> FR(3), FL(3), RR(3), RL(3);
+
+  int loc = 0;
+  for (auto&& joint : robot.joints()) {
+    if (joint->name().find("lower") != std::string::npos) {
+      loc = 2;
+    }
+    if (joint->name().find("upper") != std::string::npos) {
+      loc = 1;
+    }
+    if (joint->name().find("hip") != std::string::npos) {
+      loc = 0;
+    }
+    if (joint->name().find("FR") != std::string::npos) {
+      FR[loc] = joint;
+    }
+    if (joint->name().find("FL") != std::string::npos) {
+      FL[loc] = joint;
+    }
+    if (joint->name().find("RR") != std::string::npos) {
+      RR[loc] = joint;
+    }
+    if (joint->name().find("RL") != std::string::npos) {
+      RL[loc] = joint;
+    }
+  }
+
+  std::vector<std::vector<JointSharedPtr>> chain_joints{FL, FR, RL, RR};
+
+  return chain_joints;
+}
+
+Chain BuildChain(std::vector<JointSharedPtr>& joints) {
+  auto j0 = joints[0];
+  auto j1 = joints[1];
+  auto j2 = joints[2];
+
+  // Calculate all relevant relative poses.
+  Pose3 M_T_H = j0->pMc();
+  Pose3 M_H_T = M_T_H.inverse();
+  Pose3 M_H_U = j1->pMc();
+  Pose3 M_U_H = M_H_U.inverse();
+  Pose3 M_U_L = j2->pMc();
+  Pose3 M_L_U = M_U_L.inverse();
+  Pose3 M_T_L = M_T_H * M_H_U * M_U_L;
+  Pose3 M_L_T = M_T_L.inverse();
+
+  // Create chains
+  Chain chain1(M_T_H, j0->cScrewAxis());
+  Chain chain2(M_H_U, j1->cScrewAxis());
+  Chain chain3(M_U_L, j2->cScrewAxis());
+
+  std::vector<Chain> chains{chain1, chain2, chain3};
+
+  Chain composed = Chain::compose(chains);
+
+  return composed;
+}
+
+std::vector<Chain> ChainDynamicsGraph::getComposedChains(
+    std::vector<std::vector<JointSharedPtr>>& chain_joints) {
+  Chain composed_fr, composed_fl, composed_rr, composed_rl;
+
+  composed_fl = BuildChain(chain_joints[0]);
+  composed_fr = BuildChain(chain_joints[1]);
+  composed_rl = BuildChain(chain_joints[2]);
+  composed_rr = BuildChain(chain_joints[3]);
+
+  std::vector<Chain> composed_chains{composed_fl, composed_fr, composed_rl,
+                                     composed_rr};
+
+  return composed_chains;
+}
+
+NonlinearFactorGraph ChainDynamicsGraph::dynamicsFactors(
+    const Robot &robot,const int t,
+    const std::optional<PointOnLinks> &contact_points,
+    const std::optional<double> &mu) const {
+
+    // Initialize graph
+    NonlinearFactorGraph graph;
+
+    // Set Gravity Wrench
+    Vector6 gravityMass;
+    auto graph_gravity = gravity();
+    if (graph_gravity) 
+      gravityMass << 0.0, 0.0, 0.0, *graph_gravity * trunk_mass_;
+    else
+      gravityMass << 0.0, 0.0, 0.0, 0.0, 0.0, (-9.8) * trunk_mass_;
+    Vector6_ gravity_wrench(gravityMass);
+
+    // Create expression for wrench constraint on trunk
+    Vector6_ trunk_wrench_constraint = gravity_wrench;
+
+    // Set tolerances
+
+    // Get tolerance
+    Vector6 wrench_tolerance = opt().f_cost_model->sigmas();
+    Vector3 contact_tolerance = opt().cm_cost_model->sigmas();
+
+     std::vector<Key> wrench_keys;
+
+    for (int i = 0 ; i < 4 ; ++i) {
+    
+    bool foot_in_contact = false;
+    // Get key for wrench at hip joint with id 0
+    const Key wrench_key_3i_T = WrenchKey(0, 3*i, t);
+
+    // create expression for the wrench and initialize to zero
+    Vector6_ wrench_3i_T(Vector6::Zero());
+
+    // Set wrench expression on trunk by leg, according to contact 
+    for (auto&& cp : *contact_points) {
+      if (cp.link->id() == 3*(i+1)) {
+        wrench_3i_T = Vector6_(wrench_key_3i_T);
+        foot_in_contact = true;
+      }
+    }
+
+    // add wrench to trunk constraint
+    wrench_keys.push_back(wrench_key_3i_T);
+    
+
+    // Get expression for end effector wrench using chain
+    Vector6_ wrench_end_effector =
+        composed_chains_[i].AdjointWrenchConstraint3(chain_joints_[i], wrench_key_3i_T, t);
+
+    // Create contact constraint
+    Point3 contact_in_com(0.0, 0.0, -0.07);
+    Vector3_ contact_constraint = ContactDynamicsMomentConstraint(
+        wrench_end_effector, gtsam::Pose3(gtsam::Rot3(), (-1) * contact_in_com));
+    auto contact_expression = VectorExpressionEquality<3>(contact_constraint,
+                                                           contact_tolerance);
+    if (foot_in_contact)
+    graph.add(contact_expression.createFactor(1));
+    else {
+      Vector6 wrench_zero = gtsam::Z_6x1;
+      graph.addPrior(wrench_key_3i_T, wrench_zero, opt().f_cost_model);
+    }
+  }
+
+  // Add wrench factor for trunk link
+    graph.add(
+          WrenchFactor(opt().f_cost_model, robot.link("trunk"), wrench_keys, t, gravity()));
+
+  return graph;
+}
+
+gtsam::NonlinearFactorGraph ChainDynamicsGraph::qFactors(
+    const Robot &robot, const int t,
+    const std::optional<PointOnLinks> &contact_points) const {
+  NonlinearFactorGraph graph;
+
+  // Get Pose key for base link
+  const Key base_key = PoseKey(0, t);
+
+  // Get tolerance
+  Vector6 tolerance = opt().p_cost_model->sigmas();
+
+  for (int i = 0; i < 4; ++i) {
+    // Get end effector key
+    const Key end_effector_key = PoseKey(3*(i+1), t);
+
+    // Get expression for end effector pose
+    gtsam::Vector6_ chain_expression = composed_chains_[i].Poe3Factor(
+        chain_joints_[i], base_key, end_effector_key, t);
+
+    auto chain_constraint= VectorExpressionEquality<6>(chain_expression,
+                                                           tolerance);
+
+    graph.add(chain_constraint.createFactor(1.0));
+  }
+
+  gtsam::Vector3 gravity_used;
+  auto graph_gravity = gravity();
+  if (graph_gravity)
+    gravity_used = *graph_gravity;
+  else
+    gravity_used = gtsam::Vector3(0, 0, -9.8);
+
+  // Add contact factors.
+  if (contact_points) {
+    for (auto &&cp : *contact_points) {
+      ContactHeightFactor contact_pose_factor(
+          PoseKey(cp.link->id(), t), opt().cp_cost_model, cp.point, gravity_used);
+      graph.add(contact_pose_factor);
+    }
+  }
+
+  return graph;
+}
+
+gtsam::NonlinearFactorGraph ChainDynamicsGraph::dynamicsFactorGraph(
+    const Robot &robot, const int t,
+    const std::optional<PointOnLinks> &contact_points,
+    const std::optional<double> &mu) const {
+  NonlinearFactorGraph graph;
+  graph.add(qFactors(robot, t, contact_points));
+  //graph.add(vFactors(robot, t, contact_points));
+  //graph.add(aFactors(robot, t, contact_points));
+  graph.add(dynamicsFactors(robot, t, contact_points, mu));
+  return graph;
+}
+
+} // namesapce gtdynamics
\ No newline at end of file
diff --git a/gtdynamics/dynamics/ChainDynamicsGraph.h b/gtdynamics/dynamics/ChainDynamicsGraph.h
new file mode 100644
index 00000000..84342112
--- /dev/null
+++ b/gtdynamics/dynamics/ChainDynamicsGraph.h
@@ -0,0 +1,113 @@
+/* ----------------------------------------------------------------------------
+ * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file ChainDynamicsGraph.h
+ * @brief Builds a chain dynamics graph from a Robot object.
+ * @author Dan Barladeanu
+ */
+
+#pragma once
+
+
+//#include "gtdynamics/dynamics/OptimizerSetting.h"
+#include "gtdynamics/dynamics/DynamicsGraph.h"
+#include "gtdynamics/dynamics/Chain.h"
+#include <gtdynamics/optimizer/EqualityConstraint.h>
+#include "gtdynamics/factors/ContactDynamicsMomentFactor.h"
+#include "gtdynamics/factors/WrenchFactor.h"
+#include <gtdynamics/factors/ContactHeightFactor.h>
+
+namespace gtdynamics {
+
+using gtsam::Vector3;
+using gtsam::Vector3_;
+using gtsam::Vector6;
+using gtsam::Vector6_;
+using gtsam::Double_;
+using gtsam::Pose3;
+using gtsam::Point3;
+using gtsam::Rot3;
+using gtsam::Key;
+using gtsam::NonlinearFactorGraph ;
+
+class ChainDynamicsGraph : public DynamicsGraph {
+
+  private:
+  std::vector<std::vector<JointSharedPtr>> chain_joints_;
+  std::vector<Chain> composed_chains_;
+  double trunk_mass_;
+
+  public:
+
+    /**
+     * Constructor
+     * @param  robot      the robot
+     * @param  opt          settings for optimizer
+     * @param  angle_tolerance   tolerance for angle estimation
+     * @param  torque_tolerance   tolerance for torque estimation
+     * @param  dynamics_tolerance   tolerance for dynamics estimation
+     * @param  gravity      gravity in world frame
+     * @param  planar_axis  axis of the plane, used only for planar robot
+     */
+   ChainDynamicsGraph(
+       const Robot &robot,
+       const OptimizerSetting &opt,
+       const std::optional<Vector3> &gravity = {},
+       const std::optional<Vector3> &planar_axis = {})
+       : DynamicsGraph(opt, gravity, planar_axis),
+         chain_joints_(getChainJoints(robot)),
+         composed_chains_(getComposedChains(chain_joints_)),
+         trunk_mass_(robot.link("trunk")->mass()) {}
+
+    // Destructor
+    ~ChainDynamicsGraph() {}
+
+
+  /// Return q-level nonlinear factor graph (pose related factors)
+    gtsam::NonlinearFactorGraph qFactors(
+        const Robot &robot, const int t,
+        const std::optional<PointOnLinks> &contact_points = {}) const override;
+
+   /**
+    * Create dynamics factors of the chain graph
+    * @param robot          the robot
+    * @param t              time step
+    * link and 0 denotes no contact.
+    * @param contact_points optional vector of contact points.
+    * @param mu             optional coefficient of static friction.
+    */
+   NonlinearFactorGraph dynamicsFactors(
+       const Robot &robot, const int t,
+       const std::optional<PointOnLinks> &contact_points = {},
+       const std::optional<double> &mu = {}) const override;
+
+  /**
+   * Return nonlinear factor graph of all dynamics factors
+   * @param robot          the robot
+   * @param t              time step
+   * link and 0 denotes no contact.
+   * @param contact_points optional vector of contact points.
+   * @param mu             optional coefficient of static friction.
+   */
+  gtsam::NonlinearFactorGraph dynamicsFactorGraph(
+      const Robot &robot, const int t,
+      const std::optional<PointOnLinks> &contact_points = {},
+      const std::optional<double> &mu = {}) const override;
+
+  // Get a vector of legs, each leg is a vector of its joints
+  static std::vector<std::vector<JointSharedPtr>> getChainJoints(
+      const Robot &robot);
+
+  // Get composed chains from chain joints
+  static std::vector<Chain> getComposedChains(
+      std::vector<std::vector<JointSharedPtr>> &chain_joints);
+
+};
+
+
+} // namespace gtdynamics
\ No newline at end of file
diff --git a/gtdynamics/dynamics/DynamicsGraph.h b/gtdynamics/dynamics/DynamicsGraph.h
index a9e54f1b..9490b7ff 100644
--- a/gtdynamics/dynamics/DynamicsGraph.h
+++ b/gtdynamics/dynamics/DynamicsGraph.h
@@ -109,7 +109,7 @@ class DynamicsGraph {
                               const gtsam::Values &known_values);
 
   /// Return q-level nonlinear factor graph (pose related factors)
-  gtsam::NonlinearFactorGraph qFactors(
+  virtual gtsam::NonlinearFactorGraph qFactors(
       const Robot &robot, const int t,
       const std::optional<PointOnLinks> &contact_points = {}) const;
 
@@ -124,7 +124,7 @@ class DynamicsGraph {
       const std::optional<PointOnLinks> &contact_points = {}) const;
 
   /// Return dynamics-level nonlinear factor graph (wrench related factors)
-  gtsam::NonlinearFactorGraph dynamicsFactors(
+  virtual gtsam::NonlinearFactorGraph dynamicsFactors(
       const Robot &robot, const int t,
       const std::optional<PointOnLinks> &contact_points = {},
       const std::optional<double> &mu = {}) const;
@@ -137,7 +137,7 @@ class DynamicsGraph {
    * @param contact_points optional vector of contact points.
    * @param mu             optional coefficient of static friction.
    */
-  gtsam::NonlinearFactorGraph dynamicsFactorGraph(
+  virtual gtsam::NonlinearFactorGraph dynamicsFactorGraph(
       const Robot &robot, const int t,
       const std::optional<PointOnLinks> &contact_points = {},
       const std::optional<double> &mu = {}) const;
@@ -289,6 +289,8 @@ class DynamicsGraph {
       const Robot &robot, const int t, const std::string &link_name,
       const gtsam::Pose3 &target_pose) const;
 
+  inline std::optional<gtsam::Vector3> gravity() const { return gravity_; }
+
   /**
    * Return the joint accelerations
    * @param robot the robot
diff --git a/gtdynamics/factors/ContactDynamicsMomentFactor.h b/gtdynamics/factors/ContactDynamicsMomentFactor.h
index 2b1f8aa7..118d066c 100644
--- a/gtdynamics/factors/ContactDynamicsMomentFactor.h
+++ b/gtdynamics/factors/ContactDynamicsMomentFactor.h
@@ -46,6 +46,24 @@ inline gtsam::Vector3_ ContactDynamicsMomentConstraint(
   return error;
 }
 
+/**
+ * ContactDynamicsMomentConstraint is a 3-dimensional constraint which enforces
+ * zero moment at the contact point for the link. This is an alternative
+ * interface for expressions as inputs
+ */
+inline gtsam::Vector3_ ContactDynamicsMomentConstraint(
+    gtsam::Vector6_ contact_wrench, const gtsam::Pose3 &cTcom) {
+  gtsam::Matrix36 H_contact_wrench;
+  H_contact_wrench << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0;
+
+  gtsam::Matrix36 H =
+      H_contact_wrench * cTcom.inverse().AdjointMap().transpose();
+  const std::function<gtsam::Vector3(gtsam::Vector6)> f =
+      [H](const gtsam::Vector6 &F) { return H * F; };
+  gtsam::Vector3_ error = gtsam::linearExpression(f, contact_wrench, H);
+  return error;
+}
+
 /**
  * ContactDynamicsMomentFactor is unary nonlinear factor which enforces zero
  * moment at the contact point for the link.
diff --git a/gtdynamics/universal_robot/Joint.cpp b/gtdynamics/universal_robot/Joint.cpp
index ff926268..3743c114 100644
--- a/gtdynamics/universal_robot/Joint.cpp
+++ b/gtdynamics/universal_robot/Joint.cpp
@@ -372,6 +372,20 @@ gtsam::Vector6_ Joint::twistAccelConstraint(uint64_t t) const {
   return twistAccel_c_hat1 + twistAccel_c_hat2 - twistAccel_c;
 }
 
+/* ************************************************************************* */
+gtsam::Vector6_ Joint::childAdjointWrench(gtsam::Vector6_ &wrench_p,
+                                          uint64_t t) const {
+  gtsam::Double_ q(JointAngleKey(id(), t));
+
+  gtsam::Vector6_ wrench_c_adjoint(
+      std::bind(&Joint::transformWrenchCoordinate, this, parent(),
+                std::placeholders::_1, std::placeholders::_2,
+                std::placeholders::_3, std::placeholders::_4),
+      q, wrench_p);
+
+  return wrench_c_adjoint;
+}
+
 /* ************************************************************************* */
 gtsam::Vector6_ Joint::wrenchEquivalenceConstraint(uint64_t t) const {
   gtsam::Vector6_ wrench_p(WrenchKey(parent()->id(), id(), t));
diff --git a/gtdynamics/universal_robot/Joint.h b/gtdynamics/universal_robot/Joint.h
index 5591783e..31e141a6 100644
--- a/gtdynamics/universal_robot/Joint.h
+++ b/gtdynamics/universal_robot/Joint.h
@@ -446,6 +446,12 @@ class Joint : public std::enable_shared_from_this<Joint> {
    */
   gtsam::Vector6_ wrenchEquivalenceConstraint(uint64_t t = 0) const;
 
+  /**
+   * @brief Create expression for child wrench adjoint from parent link
+   */
+  gtsam::Vector6_ childAdjointWrench(gtsam::Vector6_ &wrench_p,
+                                     uint64_t t = 0) const;
+
   /**
    * @brief Create expression that constraint the relation between
    * wrench and torque on each link.
diff --git a/gtdynamics/universal_robot/Link.h b/gtdynamics/universal_robot/Link.h
index a0180a65..76b0d502 100644
--- a/gtdynamics/universal_robot/Link.h
+++ b/gtdynamics/universal_robot/Link.h
@@ -141,6 +141,12 @@ class Link : public std::enable_shared_from_this<Link> {
   /// Return link mass.
   double mass() const { return mass_; }
 
+  // Set Mass
+  inline void setMass(const double mass) { mass_ = mass; }
+
+  // Set Inertia
+  inline void setInertia(const gtsam::Matrix3 &inertia) { inertia_ = inertia; }
+
   /// Return center of mass (gtsam::Pose3)
   const gtsam::Pose3 &centerOfMass() const { return centerOfMass_; }
 
diff --git a/gtdynamics/utils/ChainInitializer.cpp b/gtdynamics/utils/ChainInitializer.cpp
index c3dd3b62..87cab0af 100644
--- a/gtdynamics/utils/ChainInitializer.cpp
+++ b/gtdynamics/utils/ChainInitializer.cpp
@@ -27,30 +27,35 @@ gtsam::Values ChainInitializer::ZeroValues(
 
   // Initialize link dynamics to 0.
   for (auto&& link : robot.links()) {
-    int i = link->id();
-    InsertPose(&values, i, t, AddGaussianNoiseToPose(link->bMcom(), sampler));
-    InsertTwist(&values, i, t, sampler.sample());
-    InsertTwistAccel(&values, i, t, sampler.sample());
+    const int i = link->id();
+    if (i==0 || i==3 || i==6 || i==9 || i==12)
+      InsertPose(&values, i, t, AddGaussianNoiseToPose(link->bMcom(), sampler));
+    //InsertTwist(&values, i, t, sampler.sample());
+    //InsertTwistAccel(&values, i, t, sampler.sample());
   }
+    InsertTwist(&values, 0, t, sampler.sample());
+    InsertTwistAccel(&values, 0, t, sampler.sample());
 
   // Initialize joint kinematics/dynamics to 0.
   for (auto&& joint : robot.joints()) {
-    int j = joint->id();
-    if (joint->parent()->name().find("trunk") < 100) {
+    const int j = joint->id();
+    if (joint->parent()->name().find("trunk") != std::string::npos) {
       InsertWrench(&values, joint->parent()->id(), j, t, sampler.sample());
     }
-    std::vector<DynamicsSymbol> keys = {TorqueKey(j, t), JointAngleKey(j, t),
+    std::vector<DynamicsSymbol> keys = {JointAngleKey(j, t),
                                         JointVelKey(j, t), JointAccelKey(j, t)};
     for (size_t i = 0; i < keys.size(); i++)
       values.insert(keys[i], sampler.sample()[0]);
   }
 
-  if (contact_points) {
-    for (auto&& cp : *contact_points) {
+  //if (contact_points) {
+    //for (auto&& cp : *contact_points) {
       // TODO(frank): allow multiple contact points on one link, id = 0,1,2...
-      values.insert(ContactWrenchKey(cp.link->id(), 0, t), sampler.sample());
-    }
-  }
+      //values.insert(ContactWrenchKey(cp.link->id(), 0, t), sampler.sample());
+      //std::cout<<t<<std::endl;
+      //InsertPose(&values, cp.link->id(), t, AddGaussianNoiseToPose(cp.link->bMcom(), sampler));
+    //}
+  //}
 
   return values;
 }
diff --git a/gtdynamics/utils/ChainInitializer.h b/gtdynamics/utils/ChainInitializer.h
index 83c8dca2..e4771799 100644
--- a/gtdynamics/utils/ChainInitializer.h
+++ b/gtdynamics/utils/ChainInitializer.h
@@ -17,7 +17,7 @@
 
 namespace gtdynamics {
 
-class ChainInitializer : Initializer {
+class ChainInitializer : public Initializer {
 
   public:
       /**
diff --git a/tests/testChain.cpp b/tests/testChain.cpp
index aed16a44..550fd3ea 100644
--- a/tests/testChain.cpp
+++ b/tests/testChain.cpp
@@ -11,15 +11,20 @@
  * @author: Dan Barladeanu, Frank Dellaert
  */
 
-#define BOOST_BIND_NO_PLACEHOLDERS
 #include <CppUnitLite/TestHarness.h>
 #include <gtdynamics/dynamics/Chain.h>
+#include <gtdynamics/dynamics/ChainDynamicsGraph.h>
+#include <gtdynamics/factors/ContactDynamicsMomentFactor.h>
+#include <gtdynamics/factors/MinTorqueFactor.h>
 #include <gtdynamics/optimizer/EqualityConstraint.h>
+#include <gtdynamics/optimizer/Optimizer.h>
 #include <gtdynamics/universal_robot/sdf.h>
+#include <gtdynamics/utils/FootContactConstraintSpec.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/factorTesting.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 
 using namespace gtdynamics;
 using gtsam::assert_equal;
@@ -183,7 +188,7 @@ TEST(Chain, ChainConstraint) {
 
   // Create VectorExpressionEquality Constraint
   auto constraint = VectorExpressionEquality<3>(expression, tolerance);
-  Vector3 expected_values(1, 1.9, 1.3);
+  Vector3 expected_values(-1, -0.3, 0.3);  // regression
   bool constraint_violation = constraint.feasible(init_values);
   Vector values = constraint(init_values);
   EXPECT(!constraint_violation);
@@ -303,6 +308,310 @@ TEST(Chain, DynamicalEquality3_H_angles_chain1) {
   }
 }
 
+// Test Chain Constraint Jacobians -AdjointWrenchEquality3 - H_angles - first type
+// of chain
+TEST(Chain, AdjointWrenchEquality3_H_angles_chain1) {
+  // Initialize pose and screw axis for chain instantiation
+  Pose3 sMb = Pose3(Rot3(), Point3(1, 1, 1));
+  Matrix screwAxis(6, 1);
+  screwAxis << 0.0, 0.0, 1.0, 0.0, 5.0, 0.0;
+
+  // Instantiate chains and create a vector
+  Chain joint1(sMb, screwAxis), joint2(sMb, screwAxis), joint3(sMb, screwAxis);
+  std::vector<Chain> chains{joint1, joint2, joint3};
+
+  // Compose chains
+  Chain composed = Chain::compose(chains);
+
+  Vector3 angles, torques;
+  gtsam::Vector wrench_body(6);
+  Matrix J1, J;
+
+  // binded function for numerical derivative
+  auto f = std::bind(&Chain::AdjointWrenchEquality3, composed, _1, _2,
+                     nullptr, nullptr);
+
+  // lambda function to get numerical derivative
+  auto num_derivative = [&](Vector3 angles, Vector6 wrench_body) {
+    return gtsam::numericalDerivative21<Vector6, Vector3, Vector6>(
+        f, angles, wrench_body);
+  };
+
+  // lambda function to get the Jacobian
+  auto get_jacobian = [&](Vector3 angles, Vector6 wrench_body) {
+    composed.AdjointWrenchEquality3(angles, wrench_body, J1, nullptr);
+    return J1;
+  };
+
+  Matrix anglesMat = get_angles_test_cases();
+  Matrix torquesMat = get_torques_test_cases();
+  Matrix wrenchMat = get_wrench_test_cases();
+
+  for (int i = 0; i < anglesMat.rows(); ++i) {
+    angles = anglesMat.row(i);
+    wrench_body = wrenchMat.row(i);
+
+    auto numericalH_case = num_derivative(angles, wrench_body);
+
+    J = get_jacobian(angles, wrench_body);
+
+    EXPECT(assert_equal(J, numericalH_case, 1e-5));
+  }
+}
+
+// Test Chain Constraint Jacobians -AdjointWrenchEquality3 - H_angles - second type
+// of chain
+TEST(Chain, AdjointWrenchEquality3_H_angles_chain2) {
+  // Initialize pose and screw axis for chain instantiation
+  Pose3 sMb = Pose3(Rot3(), Point3(17.0, -3.0, 99.5));
+  Matrix screwAxis(6, 1);
+  double one_over_sqrt_3 = 1 / sqrt(3);
+  screwAxis << one_over_sqrt_3, one_over_sqrt_3, one_over_sqrt_3, 1.5, 1.5,
+      0.0;  // size of omega should be 1
+
+  // Instantiate chains and create a vector
+  Chain joint1(sMb, screwAxis), joint2(sMb, screwAxis), joint3(sMb, screwAxis);
+  std::vector<Chain> chains{joint1, joint2, joint3};
+
+  // Compose chains
+  Chain composed = Chain::compose(chains);
+
+  Vector3 angles, torques;
+  gtsam::Vector wrench_body(6);
+  Matrix J1, J;
+
+  // binded function for numerical derivative
+  auto f = std::bind(&Chain::AdjointWrenchEquality3, composed, _1, _2,
+                     nullptr, nullptr);
+
+  // lambda function to get numerical derivative
+  auto num_derivative = [&](Vector3 angles, Vector6 wrench_body) {
+    return gtsam::numericalDerivative21<Vector6, Vector3, Vector6>(
+        f, angles, wrench_body);
+  };
+
+  // lambda function to get the Jacobian
+  auto get_jacobian = [&](Vector3 angles, Vector6 wrench_body) {
+    composed.AdjointWrenchEquality3(angles, wrench_body, J1, nullptr);
+    return J1;
+  };
+
+  Matrix anglesMat = get_angles_test_cases();
+  Matrix torquesMat = get_torques_test_cases();
+  Matrix wrenchMat = get_wrench_test_cases();
+
+  for (int i = 0; i < anglesMat.rows(); ++i) {
+    angles = anglesMat.row(i);
+    wrench_body = wrenchMat.row(i);
+
+    auto numericalH_case = num_derivative(angles, wrench_body);
+
+    J = get_jacobian(angles, wrench_body);
+
+    EXPECT(assert_equal(J, numericalH_case, 1e-5));
+  }
+}
+
+// Test Chain Constraint Jacobians -AdjointWrenchEquality3 - H_angles - third type
+// of chain
+TEST(Chain, AdjointWrenchEquality3_H_angles_chain3) {
+  // Initialize pose and screw axis for chain instantiation
+  Pose3 sMb = Pose3(Rot3(), Point3(0, 3, 8));
+  Matrix screwAxis0(6, 1), screwAxis1(6, 1), screwAxis2(6, 1);
+  screwAxis0 << 0.0, 0.0, 1.0, 0.0, 5.0, 0.0;
+  screwAxis1 << 0.0, 1.0, 0.0, 3.0, 0.0, 0.0;
+  screwAxis2 << 1.0, 0.0, 0.0, 0.0, 0.0, 29.0;
+
+  // Instantiate chains and create a vector
+  Chain joint1(sMb, screwAxis0), joint2(sMb, screwAxis1),
+      joint3(sMb, screwAxis2);
+  std::vector<Chain> chains{joint1, joint2, joint3};
+
+  // Compose chains
+  Chain composed = Chain::compose(chains);
+
+  Vector3 angles, torques;
+  gtsam::Vector wrench_body(6);
+  Matrix J1, J;
+
+  // binded function for numerical derivative
+  auto f = std::bind(&Chain::AdjointWrenchEquality3, composed, _1, _2,
+                     nullptr, nullptr);
+
+  // lambda function to get numerical derivative
+  auto num_derivative = [&](Vector3 angles, Vector6 wrench_body) {
+    return gtsam::numericalDerivative21<Vector6, Vector3, Vector6>(
+        f, angles, wrench_body);
+  };
+
+  // lambda function to get the Jacobian
+  auto get_jacobian = [&](Vector3 angles, Vector6 wrench_body) {
+    composed.AdjointWrenchEquality3(angles, wrench_body, J1, nullptr);
+    return J1;
+  };
+
+  Matrix anglesMat = get_angles_test_cases();
+  Matrix torquesMat = get_torques_test_cases();
+  Matrix wrenchMat = get_wrench_test_cases();
+
+  for (int i = 0; i < anglesMat.rows(); ++i) {
+    angles = anglesMat.row(i);
+    wrench_body = wrenchMat.row(i);
+
+    auto numericalH_case = num_derivative(angles, wrench_body);
+
+    J = get_jacobian(angles, wrench_body);
+
+    EXPECT(assert_equal(J, numericalH_case, 1e-5));
+  }
+}
+
+// Test Chain Constraint Jacobians -PoeEquality3 - H_angles - first type
+// of chain
+TEST(Chain, PoeEquality3_H_angles_chain1) {
+  // Initialize pose and screw axis for chain instantiation
+  Pose3 sMb = Pose3(Rot3(), Point3(1, 1, 1));
+  Matrix screwAxis(6, 1);
+  screwAxis << 0.0, 0.0, 1.0, 0.0, 5.0, 0.0;
+
+  // Instantiate chains and create a vector
+  Chain joint1(sMb, screwAxis), joint2(sMb, screwAxis), joint3(sMb, screwAxis);
+  std::vector<Chain> chains{joint1, joint2, joint3};
+
+  // Compose chains
+  Chain composed = Chain::compose(chains);
+
+  Vector3 angles, torques;
+  gtsam::Vector wrench_body(6);
+  Matrix J1, J;
+
+  // binded function for numerical derivative
+  auto f = std::bind(&Chain::PoeEquality3, composed, _1, nullptr);
+
+  // lambda function to get numerical derivative
+  auto num_derivative = [&](Vector3 angles) {
+    return gtsam::numericalDerivative11<Pose3, Vector3>(
+        f, angles);
+  };
+
+  // lambda function to get the Jacobian
+  auto get_jacobian = [&](Vector3 angles) {
+    composed.PoeEquality3(angles, J1);
+    return J1;
+  };
+
+  Matrix anglesMat = get_angles_test_cases();
+
+  for (int i = 0; i < anglesMat.rows(); ++i) {
+    angles = anglesMat.row(i);
+
+    auto numericalH_case = num_derivative(angles);
+
+    J = get_jacobian(angles);
+
+    EXPECT(assert_equal(J, numericalH_case, 1e-5));
+  }
+}
+
+// Test Chain Constraint Jacobians -PoeEquality3 - H_angles - second type
+// of chain
+TEST(Chain, PoeEquality3_H_angles_chain2) {
+  // Initialize pose and screw axis for chain instantiation
+  Pose3 sMb = Pose3(Rot3(), Point3(17.0, -3.0, 99.5));
+  Matrix screwAxis(6, 1);
+  double one_over_sqrt_3 = 1 / sqrt(3);
+  screwAxis << one_over_sqrt_3, one_over_sqrt_3, one_over_sqrt_3, 1.5, 1.5,
+      0.0;  // size of omega should be 1
+
+  // Instantiate chains and create a vector
+  Chain joint1(sMb, screwAxis), joint2(sMb, screwAxis), joint3(sMb, screwAxis);
+  std::vector<Chain> chains{joint1, joint2, joint3};
+
+  // Compose chains
+  Chain composed = Chain::compose(chains);
+
+  Vector3 angles, torques;
+  gtsam::Vector wrench_body(6);
+  Matrix J1, J;
+
+  // binded function for numerical derivative
+  auto f = std::bind(&Chain::PoeEquality3, composed, _1, nullptr);
+
+  // lambda function to get numerical derivative
+  auto num_derivative = [&](Vector3 angles) {
+    return gtsam::numericalDerivative11<Pose3, Vector3>(
+        f, angles);
+  };
+
+  // lambda function to get the Jacobian
+  auto get_jacobian = [&](Vector3 angles) {
+    composed.PoeEquality3(angles, J1);
+    return J1;
+  };
+
+  Matrix anglesMat = get_angles_test_cases();
+
+  for (int i = 0; i < anglesMat.rows(); ++i) {
+    angles = anglesMat.row(i);
+
+    auto numericalH_case = num_derivative(angles);
+
+    J = get_jacobian(angles);
+
+    EXPECT(assert_equal(J, numericalH_case, 1e-5));
+  }
+}
+
+// Test Chain Constraint Jacobians -PoeEquality3 - H_angles - third type
+// of chain
+TEST(Chain, PoeEquality3_H_angles_chain3) {
+  // Initialize pose and screw axis for chain instantiation
+  Pose3 sMb = Pose3(Rot3(), Point3(0, 3, 8));
+  Matrix screwAxis0(6, 1), screwAxis1(6, 1), screwAxis2(6, 1);
+  screwAxis0 << 0.0, 0.0, 1.0, 0.0, 5.0, 0.0;
+  screwAxis1 << 0.0, 1.0, 0.0, 3.0, 0.0, 0.0;
+  screwAxis2 << 1.0, 0.0, 0.0, 0.0, 0.0, 29.0;
+
+  // Instantiate chains and create a vector
+  Chain joint1(sMb, screwAxis0), joint2(sMb, screwAxis1),
+      joint3(sMb, screwAxis2);
+  std::vector<Chain> chains{joint1, joint2, joint3};
+
+  // Compose chains
+  Chain composed = Chain::compose(chains);
+
+  Vector3 angles, torques;
+  gtsam::Vector wrench_body(6);
+  Matrix J1, J;
+
+  // binded function for numerical derivative
+  auto f = std::bind(&Chain::PoeEquality3, composed, _1, nullptr);
+
+  // lambda function to get numerical derivative
+  auto num_derivative = [&](Vector3 angles) {
+    return gtsam::numericalDerivative11<Pose3, Vector3>(
+        f, angles);
+  };
+
+  // lambda function to get the Jacobian
+  auto get_jacobian = [&](Vector3 angles) {
+    composed.PoeEquality3(angles, J1);
+    return J1;
+  };
+
+  Matrix anglesMat = get_angles_test_cases();
+
+  for (int i = 0; i < anglesMat.rows(); ++i) {
+    angles = anglesMat.row(i);
+
+    auto numericalH_case = num_derivative(angles);
+
+    J = get_jacobian(angles);
+
+    EXPECT(assert_equal(J, numericalH_case, 1e-5));
+  }
+}
+
 // Test Chain Constraint Jacobians - DynamicalEquality3 - H_angles - second type
 // of chain
 TEST(Chain, DynamicalEquality3_H_angles_chain2) {
@@ -564,7 +873,7 @@ TEST(Chain, ChainConstraintFactorJacobians) {
 
   // Create VectorExpressionEquality Constraint
   auto constraint = VectorExpressionEquality<3>(expression, tolerance);
-  Vector3 expected_values(1, 1.9, 1.3);
+  Vector3 expected_values(-1, -0.3, 0.3);  // regression
   bool constraint_violation = constraint.feasible(init_values);
   Vector values = constraint(init_values);
   EXPECT(!constraint_violation);
@@ -577,6 +886,608 @@ TEST(Chain, ChainConstraintFactorJacobians) {
   EXPECT_CORRECT_FACTOR_JACOBIANS(*factor, init_values, 1e-7, 1e-3);
 }
 
+TEST(Chain, A1QuadOneLegCompare) {
+  // This test checks equality between torque calculation with and without
+  // chains. This assumes one static leg of the a1 quadruped with zero mass for
+  // the links besides the trunk.
+
+  auto robot =
+      CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1");
+
+  // initialize joints of FL leg
+  JointSharedPtr j0, j1, j2;
+
+  for (auto&& joint : robot.joints()) {
+    if (joint->name().find("FL") != std::string::npos) {
+      if (joint->name().find("lower") != std::string::npos) {
+        j2 = joint;
+      }
+      if (joint->name().find("upper") != std::string::npos) {
+        j1 = joint;
+      }
+      if (joint->name().find("hip") != std::string::npos) {
+        j0 = joint;
+      }
+    }
+  }
+
+  // Calculate all relevant relative poses.
+  Pose3 M_T_H = j0->pMc();
+  Pose3 M_H_T = M_T_H.inverse();
+  Pose3 M_H_U = j1->pMc();
+  Pose3 M_U_H = M_H_U.inverse();
+  Pose3 M_U_L = j2->pMc();
+  Pose3 M_L_U = M_U_L.inverse();
+  Pose3 M_T_L = M_T_H * M_H_U * M_U_L;
+  Pose3 M_L_T = M_T_L.inverse();
+
+  // Set Gravity Wrench
+  Matrix gravity(1, 6);
+  gravity << 0.0, 0.0, 0.0, 0.0, 0.0, -10.0;
+
+  // Calculate all wrenches and torques without chains.
+  Matrix F_2_L = -gravity * M_T_L.AdjointMap();
+  Matrix F_1_U = F_2_L * M_L_U.AdjointMap();
+  Matrix F_0_H = F_1_U * M_U_H.AdjointMap();
+
+  // joint 0
+  Matrix tau0 = F_0_H * j0->cScrewAxis();
+  EXPECT(assert_equal(tau0(0, 0), -0.448145, 1e-6));  // regression
+
+  // joint 1
+  Matrix tau1 = F_1_U * j1->cScrewAxis();
+  EXPECT(assert_equal(tau1(0, 0), 1.70272, 1e-5));  // regression
+
+  // joint 2
+  Matrix tau2 = F_2_L * j2->cScrewAxis();
+  EXPECT(assert_equal(tau2(0, 0), 1.70272, 1e-5));  // regression
+
+  // Calculate all wrenches and torques with chains.
+  Chain chain1(M_T_H, j0->cScrewAxis());
+  Chain chain2(M_H_U, j1->cScrewAxis());
+  Chain chain3(M_U_L, j2->cScrewAxis());
+
+  std::vector<Chain> chains{chain1, chain2, chain3};
+
+  // Compose Chains
+  Chain composed = Chain::compose(chains);
+
+  // test for same values
+  Matrix torques = F_2_L * composed.axes();
+  EXPECT(assert_equal(torques(0, 0), tau0(0, 0), 1e-6));
+  EXPECT(assert_equal(torques(0, 1), tau1(0, 0), 1e-5));
+  EXPECT(assert_equal(torques(0, 2), tau2(0, 0), 1e-5));
+}
+
+gtsam::Values OldGraphOneLeg() {
+  auto robot =
+      CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1");
+
+  // Get trunk with one leg, only the trunk has mass
+  for (auto&& link : robot.links()) {
+    if (link->id() > 3) {
+      robot.removeLink(link);
+    } else if (link->name().find("trunk") == std::string::npos) {
+      link->setMass(0.0);
+      link->setInertia(gtsam::Matrix3::Zero());
+    } else {
+      link->setMass(1.0);
+      link->setInertia(gtsam::Matrix3::Identity());
+    }
+  }
+
+  // Create Contact Point
+  std::vector<LinkSharedPtr> lower_feet = {robot.link("FL_lower")};
+  const Point3 contact_in_com(0, 0, -0.07);
+  auto stationary =
+      std::make_shared<FootContactConstraintSpec>(lower_feet, contact_in_com);
+  auto contact_points = stationary->contactPoints();
+
+  gtsam::Vector3 gravity(0, 0, -10.0);
+
+  OptimizerSetting opt(1e-4, 1e-4, 1e-4, 1e-4);
+  DynamicsGraph graph_builder(opt, gravity);
+
+  gtsam::Vector6 wrench_zero = gtsam::Z_6x1;
+
+  gtsam::NonlinearFactorGraph graph;
+
+  // Need the following to use the wrench factor (Coriolis, Generalized
+  // Momentum, Gravity)
+  auto bp_cost_model(gtsam::noiseModel::Isotropic::Sigma(6, 1e-30));
+  graph.addPrior(PoseKey(0, 0), robot.link("trunk")->bMcom(), bp_cost_model);
+  graph.addPrior(TwistKey(0, 0), wrench_zero, bp_cost_model);
+  graph.addPrior(TwistAccelKey(0, 0), wrench_zero, bp_cost_model);
+
+  // Build dynamics factors
+  //graph.add(graph_builder.qFactors(robot, 0));//, contact_points));
+  graph.add(graph_builder.dynamicsFactors(robot, 0, contact_points, 1.0));
+
+  // Initialize joint kinematics/dynamics to 0.
+  gtsam::Values init_vals;
+  for (auto&& joint : robot.joints()) {
+    int j = joint->id();
+    InsertWrench(&init_vals, joint->parent()->id(), j, 0, wrench_zero);
+    InsertWrench(&init_vals, joint->child()->id(), j, 0, wrench_zero);
+    InsertTorque(&init_vals, j, 0, 0.0);
+    InsertJointAngle(&init_vals, j, 0, 0.0);
+  }
+  for (auto&& link : robot.links()) {
+    //graph.addPrior(PoseKey(link->id(), 0), link->bMcom(), bp_cost_model);
+    InsertTwist(&init_vals, link->id(), 0, wrench_zero);
+    InsertTwistAccel(&init_vals, link->id(), 0, wrench_zero);
+    InsertPose(&init_vals, link->id(), 0, link->bMcom());
+  }
+  init_vals.insert(ContactWrenchKey(3, 0, 0), wrench_zero);
+
+  // Constraint angles to zero
+  Optimizer optimizer;
+  EqualityConstraints eqs;
+  auto cost_model = gtsam::noiseModel::Unit::Create(1);
+  for (auto&& joint : robot.joints()) {
+    const int joint_id = joint->id();
+    gtsam::Double_ angle(JointAngleKey(joint_id, 0));
+    //eqs.emplace_shared<DoubleExpressionEquality>(angle, 1e-1);
+  }
+
+  /// Solve the constraint problem with LM optimizer.
+  gtsam::Values results = optimizer.optimize(graph, eqs, init_vals);
+
+  return results;
+}
+
+gtsam::Values NewGraphOneLeg() {
+  // This Graph uses chain constraints for one leg of the a1 robot, a wrench
+  // constraint on the trunk and zero angles at the joints constraints (robot
+  // standing still)
+
+  auto robot =
+      CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1");
+
+  // Get joint and composed chains for each leg
+  auto chain_joints = ChainDynamicsGraph::getChainJoints(robot);
+  auto composed_chains = ChainDynamicsGraph::getComposedChains(chain_joints);
+
+  // Initialize Constraints
+  EqualityConstraints constraints;
+
+  // Set Gravity Wrench
+  gtsam::Vector6 gravity;
+  gravity << 0.0, 0.0, 0.0, 0.0, 0.0, -10.0;
+  gtsam::Vector6_ gravity_wrench(gravity);
+
+  // Create expression for wrench constraint on trunk
+  gtsam::Vector6_ trunk_wrench_constraint = gravity_wrench;
+
+  // Set tolerances
+  double dynamicsTolerance = 6.2*(1e-5);
+  gtsam::Vector3 contact_tolerance = Vector3::Ones() * dynamicsTolerance;
+  gtsam::Vector6 wrench_tolerance = Vector6::Ones() * dynamicsTolerance;
+
+  // Get key for wrench at hip joint with id 0
+  const gtsam::Key wrench_key_0_T = gtdynamics::WrenchKey(0, 0, 0);
+
+  // create expression for the wrench
+  gtsam::Vector6_ wrench_0_T(wrench_key_0_T);
+
+  // add wrench to trunk constraint
+  trunk_wrench_constraint += wrench_0_T;
+
+  // Add trunk wrench constraint to constraints
+  constraints.emplace_shared<VectorExpressionEquality<6>>(
+      trunk_wrench_constraint, wrench_tolerance);
+
+  // Get expression for chain on the leg
+  gtsam::Vector6_ wrench_end_effector =
+      composed_chains[0].AdjointWrenchConstraint3(chain_joints[0], wrench_key_0_T, 0);
+
+  // Create contact constraint
+  Point3 contact_in_com(0.0, 0.0, -0.07);
+  gtsam::Vector3_ contact_constraint = ContactDynamicsMomentConstraint(
+      wrench_end_effector, gtsam::Pose3(gtsam::Rot3(), (-1) * contact_in_com));
+  constraints.emplace_shared<VectorExpressionEquality<3>>(contact_constraint,
+                                                          contact_tolerance);
+
+  // Create initial values.
+  gtsam::Values init_values;
+  for (auto&& joint : robot.joints()) {
+    if (joint->id() > 2) continue;
+    InsertJointAngle(&init_values, joint->id(), 0, 0.0);
+  }
+  gtsam::Vector6 wrench_zero = gtsam::Z_6x1;
+  init_values.insert(gtdynamics::WrenchKey(0, 0, 0), wrench_zero);
+
+  /// Solve the constraint problem with LM optimizer.
+  gtsam::NonlinearFactorGraph graph;
+  Optimizer optimizer;
+  gtsam::Values results = optimizer.optimize(graph, constraints, init_values);
+  return results;
+}
+
+TEST(Chain, oneLegCompareGraphsA1) {
+  auto robot =
+      CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1");
+
+  // Get joint and composed chains for each leg
+  auto chain_joints = ChainDynamicsGraph::getChainJoints(robot);
+
+  // calculate both ways
+  gtsam::Values new_graph_results = NewGraphOneLeg();
+  gtsam::Values old_graph_results = OldGraphOneLeg();
+
+  // Get torque and angle results and compare
+  for (int i = 0; i < 3; ++i) {
+    double new_angle =
+        new_graph_results.at<double>(gtdynamics::JointAngleKey(i));
+    double old_angle =
+        old_graph_results.at<double>(gtdynamics::JointAngleKey(i));
+    EXPECT(assert_equal(new_angle, old_angle, 1e-4));
+  }
+
+  // Get new angles
+  double new_angle0 =
+      new_graph_results.at<double>(gtdynamics::JointAngleKey(0));
+  double new_angle1 =
+      new_graph_results.at<double>(gtdynamics::JointAngleKey(1));
+  double new_angle2 =
+      new_graph_results.at<double>(gtdynamics::JointAngleKey(2));
+
+  // Get wrench keys
+  const gtsam::Key wrench_key_0_T = gtdynamics::WrenchKey(0, 0, 0);
+  const gtsam::Key contact_wrench_key = gtdynamics::ContactWrenchKey(3, 0, 0);
+
+  // Get result wrench for hip joints and compare
+  gtsam::Vector6 wrench_new_0_T =
+      new_graph_results.at<gtsam::Vector6>(wrench_key_0_T);
+  gtsam::Vector6 wrench_old_0_T =
+      old_graph_results.at<gtsam::Vector6>(wrench_key_0_T);
+  EXPECT(assert_equal(wrench_new_0_T, wrench_old_0_T, 1e-10));
+
+  // Get result wrench for lower link and compare
+  Pose3 trunkThip = chain_joints[0][0]->parentTchild(new_angle0);
+  Pose3 hipTupper = chain_joints[0][1]->parentTchild(new_angle1);
+  Pose3 upperTlower = chain_joints[0][2]->parentTchild(new_angle2);
+  Pose3 trunkTlower = trunkThip * hipTupper * upperTlower;
+  gtsam::Vector6 wrench_new_3_L =
+      wrench_new_0_T.transpose() * trunkTlower.AdjointMap();
+  gtsam::Vector6 wrench_old_3_L =
+      old_graph_results.at<gtsam::Vector6>(contact_wrench_key);
+  EXPECT(assert_equal(wrench_new_3_L, wrench_old_3_L, 1e-3));
+
+  // calculate wrench on ground and compare
+  Point3 contact_in_com(0.0, 0.0, -0.07);
+  Pose3 M_L_G = Pose3(Rot3(), contact_in_com);
+  gtsam::Vector6 wrench_new_3_G =
+      wrench_new_3_L.transpose() * M_L_G.AdjointMap();
+  gtsam::Vector6 wrench_old_3_G =
+      wrench_old_3_L.transpose() * M_L_G.AdjointMap();
+  EXPECT(assert_equal(wrench_new_3_G, wrench_old_3_G, 1e-3));
+
+  // check torque zero on ground wrench
+  gtsam::Matrix36 H_contact_wrench;
+  H_contact_wrench << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0;
+  gtsam::Vector3 zero_torque{0.0, 0.0, 0.0};
+  gtsam::Vector3 contact_torque_new = H_contact_wrench * wrench_new_3_G;
+  gtsam::Vector3 contact_torque_old = H_contact_wrench * wrench_old_3_G;
+  EXPECT(assert_equal(contact_torque_new, zero_torque, 1e-4));
+  EXPECT(assert_equal(contact_torque_old, zero_torque, 1e-4));
+}
+
+gtsam::Values OldGraphFourLegs() {
+  auto robot =
+      CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1");
+
+  // set masses and inertias
+  for (auto&& link : robot.links()) {
+    if (link->name().find("trunk") == std::string::npos) {
+      link->setMass(0.0);
+      link->setInertia(gtsam::Matrix3::Zero());
+    }
+  }
+
+  // Create Contact Point
+  std::vector<LinkSharedPtr> lower_feet = {
+      robot.link("FL_lower"), robot.link("FR_lower"), robot.link("RL_lower"),
+      robot.link("RR_lower")};
+  const Point3 contact_in_com(0, 0, -0.07);
+  auto stationary =
+      std::make_shared<FootContactConstraintSpec>(lower_feet, contact_in_com);
+  auto contact_points = stationary->contactPoints();
+
+  gtsam::Vector3 gravity(0, 0, -10.0);
+
+  OptimizerSetting opt(5e-5, 5e-5, 5e-5, 5e-5);
+  DynamicsGraph graph_builder(opt, gravity);
+
+  gtsam::Vector6 wrench_zero = gtsam::Z_6x1;
+
+  gtsam::NonlinearFactorGraph graph;
+
+  // Need the following to use the wrench factor (Coriolis, Generalized
+  // Momentum, Gravity)
+  auto constrained_model = gtsam::noiseModel::Constrained::All(6);
+  auto bp_cost_model(gtsam::noiseModel::Isotropic::Sigma(6, 1e-5));
+  graph.addPrior(PoseKey(0, 0), robot.link("trunk")->bMcom(), constrained_model);
+  graph.addPrior(TwistKey(0, 0), wrench_zero, constrained_model);
+  graph.addPrior(TwistAccelKey(0, 0), wrench_zero, constrained_model);
+  //graph.addPrior(WrenchKey(0,0,0), wrench_zero,  bp_cost_model);
+  graph.addPrior(WrenchKey(0,3,0), wrench_zero,  constrained_model);
+  graph.addPrior(WrenchKey(0,6,0), wrench_zero,  constrained_model);
+  graph.addPrior(WrenchKey(0,9,0), wrench_zero,  constrained_model);
+  graph.addPrior(JointAngleKey(1,0), 0.44, gtsam::noiseModel::Isotropic::Sigma(1, 5e-5));
+  graph.addPrior(JointAngleKey(2,0), 0.0, gtsam::noiseModel::Isotropic::Sigma(1, 5e-5));
+ 
+  // Build dynamics factors
+  //graph.add(graph_builder.qFactors(robot, 0));//, contact_points));
+  graph.add(graph_builder.dynamicsFactors(robot, 0, contact_points, 1.0));
+
+  // Initialize joint kinematics/dynamics to 0.
+  gtsam::Values init_vals;
+  for (auto&& joint : robot.joints()) {
+    const int j = joint->id();
+    InsertWrench(&init_vals, joint->parent()->id(), j, 0, wrench_zero);
+    InsertWrench(&init_vals, joint->child()->id(), j, 0, wrench_zero);
+    InsertTorque(&init_vals, j, 0, 0.0);
+    InsertJointAngle(&init_vals, j, 0, 0.0);
+    //graph.addPrior(JointAngleKey(joint->id(),0), 0.0,  gtsam::noiseModel::Isotropic::Sigma(1, 5e-5));
+  }
+  for (auto&& link : robot.links()) {
+    InsertTwist(&init_vals, link->id(), 0, wrench_zero);
+    InsertTwistAccel(&init_vals, link->id(), 0, wrench_zero);
+    InsertPose(&init_vals, link->id(), 0, link->bMcom());
+    //graph.addPrior(PoseKey(link->id(), 0), link->bMcom(), bp_cost_model);
+    //graph.addPrior(TwistKey(link->id(), 0), wrench_zero, bp_cost_model);
+    //graph.addPrior(TwistAccelKey(link->id(), 0), wrench_zero, bp_cost_model);
+  }
+  init_vals.insert(ContactWrenchKey(3, 0, 0), wrench_zero);
+  init_vals.insert(ContactWrenchKey(6, 0, 0), wrench_zero);
+  init_vals.insert(ContactWrenchKey(9, 0, 0), wrench_zero);
+  init_vals.insert(ContactWrenchKey(12, 0, 0), wrench_zero);
+
+  /// Solve the constraint problem with LM optimizer.
+  OptimizationParameters params;
+  auto optimizer = Optimizer(params);
+  EqualityConstraints constraints;
+  gtsam::Values results = optimizer.optimize(graph, constraints, init_vals);
+
+  return results;
+}
+
+gtsam::Values NewGraphFourLegs() {
+  // This Graph uses chain constraints for four legs of the a1 robot, a wrench
+  // constraint on the trunk and zero angles and torques at the joints
+  // constraints (robot standing still)
+
+  auto robot =
+      CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1");
+
+  std::vector<LinkSharedPtr> lower_feet = {
+      robot.link("FL_lower"), robot.link("FR_lower"), robot.link("RL_lower"),
+      robot.link("RR_lower")};
+  const Point3 contact_in_com(0, 0, -0.07);
+  auto stationary =
+      std::make_shared<FootContactConstraintSpec>(lower_feet, contact_in_com);
+  auto contact_points = stationary->contactPoints();
+
+  gtsam::NonlinearFactorGraph graph;
+  gtsam::Vector3 gravity(0, 0, -10.0);
+
+  OptimizerSetting opt(5e-5, 5e-5, 5e-5, 5e-5);
+  //ChainDynamicsGraph chain_graph(robot, opt, 1*(1e-4),  6*(1e-5),  10.08*(1e-5), gravity);
+  ChainDynamicsGraph chain_graph(robot, opt, gravity);
+
+  gtsam::Vector6 wrench_zero = gtsam::Z_6x1;
+
+  auto constrained_model = gtsam::noiseModel::Constrained::All(6);
+  auto bp_cost_model(gtsam::noiseModel::Isotropic::Sigma(6, 1e-5));
+  graph.addPrior(PoseKey(0, 0), robot.link("trunk")->bMcom(), constrained_model);
+  graph.addPrior(TwistKey(0, 0), wrench_zero, constrained_model);
+  graph.addPrior(TwistAccelKey(0, 0), wrench_zero, constrained_model);
+  //graph.addPrior(WrenchKey(0,0,0), wrench_zero, bp_cost_model);
+  graph.addPrior(WrenchKey(0,3,0), wrench_zero, constrained_model);
+  graph.addPrior(WrenchKey(0,6,0), wrench_zero, constrained_model);
+  graph.addPrior(WrenchKey(0,9,0), wrench_zero, constrained_model);
+  graph.addPrior(JointAngleKey(1,0), 0.44, gtsam::noiseModel::Isotropic::Sigma(1, 5e-5));
+  graph.addPrior(JointAngleKey(2,0), 0.0, gtsam::noiseModel::Isotropic::Sigma(1, 5e-5));
+
+  // Build dynamics factors
+  //graph.add(chain_graph.qFactors(robot, 0));//, contact_points));
+  graph.add(chain_graph.dynamicsFactors(robot, 0, contact_points, 1.0));
+
+  // Create initial values.
+  gtsam::Values init_values;
+  for (auto&& joint : robot.joints()) {
+    InsertJointAngle(&init_values, joint->id(), 0, 0.0);
+    //graph.addPrior(JointAngleKey(joint->id(),0), 0.0, gtsam::noiseModel::Isotropic::Sigma(1, 5e-5));
+    //InsertTorque(&init_values, joint->id(), 0, 0.0);
+  }
+
+  for (int i = 0; i < 4; ++i) {
+    init_values.insert(gtdynamics::WrenchKey(0, 3 * i, 0), wrench_zero);
+  }
+
+  InsertPose(&init_values, 0, 0, robot.link("trunk")->bMcom());
+  InsertTwist(&init_values, 0, 0, wrench_zero);
+  InsertTwistAccel(&init_values, 0, 0, wrench_zero);
+
+  /// Solve the constraint problem with LM optimizer.
+  OptimizationParameters params;
+  auto optimizer = Optimizer(params);
+  gtsam::Values results = optimizer.optimize(graph, init_values);
+  return results;
+}
+
+TEST(Chain, fourLegsCompareGraphsA1) {
+  auto robot =
+      CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1");
+
+  // Get joint and composed chains for each leg
+  auto chain_joints = ChainDynamicsGraph::getChainJoints(robot);
+
+  // calculate both ways
+  gtsam::Values new_graph_results = NewGraphFourLegs();
+  gtsam::Values old_graph_results = OldGraphFourLegs();
+
+  // Get torque and angle results and compare
+  for (int i = 0; i < 12; ++i) {
+    double new_angle =
+        new_graph_results.at<double>(gtdynamics::JointAngleKey(i));
+    double old_angle =
+        old_graph_results.at<double>(gtdynamics::JointAngleKey(i));
+    EXPECT(assert_equal(new_angle, old_angle, 1e-3));
+  }
+
+  int hip_joint_id = 0;
+  int hip_link_id = 1;
+  for (int i = 0; i < 4; ++i) {
+    // Get new angles
+    double new_angle0 =
+        new_graph_results.at<double>(gtdynamics::JointAngleKey(hip_joint_id));
+    double new_angle1 = new_graph_results.at<double>(
+        gtdynamics::JointAngleKey(hip_joint_id + 1));
+    double new_angle2 = new_graph_results.at<double>(
+        gtdynamics::JointAngleKey(hip_joint_id + 2));
+
+    // Get wrench keys
+    const gtsam::Key wrench_key_0_T = gtdynamics::WrenchKey(0, hip_joint_id, 0);
+    const gtsam::Key contact_wrench_key =
+        gtdynamics::ContactWrenchKey(hip_link_id + 2, 0, 0);
+
+    // Get result wrench for hip joints and compare
+    gtsam::Vector6 wrench_new_0_T =
+        new_graph_results.at<gtsam::Vector6>(wrench_key_0_T);
+    gtsam::Vector6 wrench_old_0_T =
+        old_graph_results.at<gtsam::Vector6>(wrench_key_0_T);
+    EXPECT(assert_equal(wrench_new_0_T, wrench_old_0_T, 1e-3));
+
+    // Get result wrench for lower link and compare
+    Pose3 trunkThip = chain_joints[i][0]->parentTchild(new_angle0);
+    Pose3 hipTupper = chain_joints[i][1]->parentTchild(new_angle1);
+    Pose3 upperTlower = chain_joints[i][2]->parentTchild(new_angle2);
+    Pose3 trunkTlower = trunkThip * hipTupper * upperTlower;
+    gtsam::Vector6 wrench_new_3_L =
+        wrench_new_0_T.transpose() * trunkTlower.AdjointMap();
+    gtsam::Vector6 wrench_old_3_L =
+        old_graph_results.at<gtsam::Vector6>(contact_wrench_key);
+    EXPECT(assert_equal(wrench_new_3_L, wrench_old_3_L, 5e-2));
+
+    // calculate wrench on ground and compare
+    Point3 contact_in_com(0.0, 0.0, -0.07);
+    Pose3 M_L_G = Pose3(Rot3(), contact_in_com);
+    gtsam::Vector6 wrench_new_3_G =
+        wrench_new_3_L.transpose() * M_L_G.AdjointMap();
+    gtsam::Vector6 wrench_old_3_G =
+        wrench_old_3_L.transpose() * M_L_G.AdjointMap();
+    EXPECT(assert_equal(wrench_new_3_G, wrench_old_3_G, 5e-2));
+
+    // check torque zero on ground wrench
+    gtsam::Matrix36 H_contact_wrench;
+    H_contact_wrench << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0;
+    gtsam::Vector3 zero_torque{0.0, 0.0, 0.0};
+    gtsam::Vector3 contact_torque_new = H_contact_wrench * wrench_new_3_G;
+    gtsam::Vector3 contact_torque_old = H_contact_wrench * wrench_old_3_G;
+    EXPECT(assert_equal(contact_torque_new, zero_torque, 1e-3));
+    EXPECT(assert_equal(contact_torque_old, zero_torque, 1e-3));
+    hip_joint_id += 3;
+    hip_link_id += 3;
+  }
+}
+
+TEST(Chain,A1forwardKinematicsWithPOE)  {
+  auto robot =
+      CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1");
+
+  OptimizerSetting opt;
+  ChainDynamicsGraph chain_graph(robot, opt);
+  auto chain_joints = chain_graph.getChainJoints(robot);
+  auto composed_chains = chain_graph.getComposedChains(chain_joints);
+
+  Vector3 test_angles( 0.0,  0.0, 0.0);
+  Pose3 chain_ee_pose = composed_chains[0].poe(test_angles, {}, nullptr);
+
+  gtsam::Values test_values;
+  InsertJointAngle(&test_values, 0, 0, 0.0);
+  InsertJointAngle(&test_values, 1, 0, 0.0);
+  InsertJointAngle(&test_values, 2, 0, 0.0);
+  gtsam::Values expected = robot.forwardKinematics(test_values, 0, robot.link("trunk")->name());
+  auto fk_ee_pose = Pose(expected, 3, 0);
+  EXPECT(assert_equal(chain_ee_pose, fk_ee_pose, 1e-6));
+}
+
+TEST(Chain, fourLegsCompareQfactorsA1)  {
+
+  auto robot =
+      CreateRobotFromFile(kUrdfPath + std::string("/a1/a1.urdf"), "a1");
+
+  std::vector<LinkSharedPtr> lower_feet = {
+      robot.link("FL_lower"), robot.link("FR_lower"), robot.link("RL_lower"),
+      robot.link("RR_lower")};
+  const Point3 contact_in_com(0, 0, -0.07);
+  auto stationary =
+      std::make_shared<FootContactConstraintSpec>(lower_feet, contact_in_com);
+  auto contact_points = stationary->contactPoints();
+
+  gtsam::NonlinearFactorGraph graph_DG, graph_CDG;
+  gtsam::Vector3 gravity(0, 0, -10.0);
+
+  OptimizerSetting opt(1e-3);
+  opt.p_cost_model = gtsam::noiseModel::Constrained::All(6);
+  ChainDynamicsGraph chain_graph(robot, opt, gravity);
+  DynamicsGraph dynamics_graph(opt, gravity );
+
+  auto bp_cost_model(gtsam::noiseModel::Isotropic::Sigma(6, 1e-6));
+  auto p_cost_model(gtsam::noiseModel::Isotropic::Sigma(6, 1e-3));
+  auto cp_cost_model(gtsam::noiseModel::Isotropic::Sigma(1, 1e-3));
+  auto cp_cost_model_angs(gtsam::noiseModel::Isotropic::Sigma(1, 5e-1));
+ 
+  graph_DG.addPrior(PoseKey(0, 0), robot.links()[0]->bMcom(), bp_cost_model);
+  graph_CDG.addPrior(PoseKey(0, 0), robot.links()[0]->bMcom(), bp_cost_model);
+  for (auto&& joint : robot.joints()){
+    const int id = joint->id();
+    graph_DG.addPrior(JointAngleKey(id, 0), 0.0, cp_cost_model_angs);
+    graph_CDG.addPrior(JointAngleKey(id, 0), 0.0, cp_cost_model_angs);
+  }
+
+  graph_CDG.add(chain_graph.qFactors(robot, 0, contact_points));
+  graph_DG.add(dynamics_graph.qFactors(robot, 0, contact_points));
+
+  // Create initial values.
+  gtsam::Values init_values_DG, init_values_CDG;
+  for (auto&& joint : robot.joints()) {
+    InsertJointAngle(&init_values_DG, joint->id(), 0, 0.0);
+    InsertJointAngle(&init_values_CDG, joint->id(), 0, 0.0);
+  }
+
+  for (auto&& link : robot.links()) {
+    const int i = link->id();
+    InsertPose(&init_values_DG, i, 0, link->bMcom());
+    if (i==0 || i==3 || i==6 || i==9 || i==12)
+      InsertPose(&init_values_CDG, i, 0, link->bMcom());
+  }
+
+  /// Solve the constraint problem with LM optimizer.
+  gtsam::LevenbergMarquardtParams params;
+  //params.setVerbosityLM("SUMMARY");
+  params.setlambdaInitial(1e10);
+  params.setlambdaLowerBound(1e-7);
+  params.setlambdaUpperBound(1e10);
+  params.setAbsoluteErrorTol(1e-3);
+  gtsam::LevenbergMarquardtOptimizer optimizer1(graph_DG, init_values_DG, params);
+  gtsam::LevenbergMarquardtOptimizer optimizer2(graph_CDG, init_values_CDG, params);
+
+  gtsam::Values results_DG = optimizer1.optimize();
+  gtsam::Values results_CDG = optimizer2.optimize();
+
+  for (int i = 0; i < 12; ++i) {
+    double angle_DG =
+        results_DG.at<double>(gtdynamics::JointAngleKey(i));
+    double angle_CDG =
+       results_CDG.at<double>(gtdynamics::JointAngleKey(i));
+    EXPECT(assert_equal(angle_DG, angle_CDG, 1e-3));
+  }
+}
+
+
 int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);