Fix all the scripts

gtdynamics/utils/Initializer.cpp

@@ -365,6 +365,8 @@ Values Initializer::ZeroValues(
     int j = joint->id();
     InsertWrench(&values, joint->parent()->id(), j, t, sampler.sample());
     InsertWrench(&values, joint->child()->id(), j, t, sampler.sample());
+
+    // Joint angles, velocities, accelerations, and torques.
     std::vector<DynamicsSymbol> keys = {TorqueKey(j, t), JointAngleKey(j, t),
                                         JointVelKey(j, t), JointAccelKey(j, t)};
     for (size_t i = 0; i < keys.size(); i++)

scripts/alejandro_yetong_01_id_four_bar.cpp

@@ -27,7 +27,7 @@ int main(int argc, char** argv) {
   using four_bar_linkage_pure::planar_axis;
   auto robot = four_bar_linkage_pure::getRobot();
 
-  gtsam::Values joint_angles_vels_accels;
+  Initializer initializer;
 
   gtsam::Vector3 gravity(0, -10, 0);
   robot = robot.fixLink("l1");
@@ -42,7 +42,7 @@ int main(int argc, char** argv) {
       graph_builder.dynamicsFactorGraph(robot, 0);
 
   // Inverse dynamics priors. We care about the torques.
-  gtsam::Vector joint_accels = gtsam::Vector::Zero(robot.numJoints());
+  gtsam::Values joint_angles_vels_accels = initializer.ZeroValues(robot, 0);
   gtsam::NonlinearFactorGraph prior_factors =
       graph_builder.inverseDynamicsPriors(robot, 0, joint_angles_vels_accels);
 
@@ -52,8 +52,7 @@ int main(int argc, char** argv) {
     prior_factors.addPrior(PoseKey(i, 0), link->bMcom(),
                            gtsam::noiseModel::Constrained::All(6));
     prior_factors.addPrior<gtsam::Vector6>(
-        TwistKey(i, 0), gtsam::Z_6x1,
-        gtsam::noiseModel::Constrained::All(6));
+        TwistKey(i, 0), gtsam::Z_6x1, gtsam::noiseModel::Constrained::All(6));
   }
   graph.add(prior_factors);
 
@@ -64,7 +63,6 @@ int main(int argc, char** argv) {
                         gtsam::noiseModel::Gaussian::Covariance(gtsam::I_1x1)));
 
   // Initialize solution.
-  Initializer initializer;
   gtsam::Values init_values = initializer.ZeroValues(robot, 0);
 
   std::cout << "\033[1;32;7mFactor Graph Optimization:\033[0m" << std::endl;

scripts/gerry00_ForwardDynamicsPrismatic.cpp

@@ -54,9 +54,11 @@ int main(int argc, char** argv) {
   InsertJointVel(&theta_and_theta_dot, j2, 0, 0.1);
   InsertJointVel(&theta_and_theta_dot, j3, 0, 0.0);
 
-  std::vector<gtsam::Vector> taus;
+  // Insert torque values for all timesteps
   for (int t = 0; t <= T; t++) {
-    taus.push_back(gtsam::Vector::Zero(3));
+    InsertTorque(&theta_and_theta_dot, j1, t, 0.0);
+    InsertTorque(&theta_and_theta_dot, j2, t, 0.0);
+    InsertTorque(&theta_and_theta_dot, j3, t, 0.0);
   }
   auto fd_priors =
       graph_builder.trajectoryFDPriors(simple_rpr, T, theta_and_theta_dot);

scripts/nithya00_constrained_opt_benchmark.cpp

@@ -76,6 +76,7 @@ int main(int argc, char** argv) {
   };
 
   /// Write results to files for plotting.
+  std::cout << "Writing resutls to penalty_data.txt and augl_data.txt for plotting" << std::endl;
   std::ofstream penalty_file;
   penalty_file.open("penalty_data.txt");
   for (size_t i = 0; i < penalty_info.num_iters.size(); i++) {

scripts/stephanie01_ForwardDynamicsThreeLink.cpp

@@ -23,6 +23,8 @@ int main(int argc, char **argv) {
   // Load the three-link robot using the relevant namespace from RobotModels.
   auto robot = simple_rr::getRobot();
 
+  Initializer initializer;
+
   // Build the factor graph for the robot.
   robot = robot.fixLink("link_0");
   gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, -9.8).finished();
@@ -32,13 +34,11 @@ int main(int argc, char **argv) {
   auto graph = graph_builder.dynamicsFactorGraph(robot, 0);
 
   // Add forward dynamics priors to factor graph.
-  gtsam::Values values;
-
+  gtsam::Values values = initializer.ZeroValues(robot, 0);
   auto priorFactors = graph_builder.forwardDynamicsPriors(robot, 0, values);
   graph.add(priorFactors);
 
   // Generate initial values to be passed in to the optimization function.
-  Initializer initializer;
   auto init_values = initializer.ZeroValues(robot, 0);
 
   // Compute forward dynamics.