From 0d4df42bf88a871a7f017c82f9729d5a80bd095c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 8 Oct 2023 20:53:57 -0400 Subject: [PATCH 1/2] fix all the scripts --- scripts/alejandro_yetong_01_id_four_bar.cpp | 12 ++++++++++-- scripts/gerry00_ForwardDynamicsPrismatic.cpp | 6 ++++-- scripts/nithya00_constrained_opt_benchmark.cpp | 1 + scripts/stephanie01_ForwardDynamicsThreeLink.cpp | 5 +++++ 4 files changed, 20 insertions(+), 4 deletions(-) diff --git a/scripts/alejandro_yetong_01_id_four_bar.cpp b/scripts/alejandro_yetong_01_id_four_bar.cpp index ae5439b24..df20fbbcd 100644 --- a/scripts/alejandro_yetong_01_id_four_bar.cpp +++ b/scripts/alejandro_yetong_01_id_four_bar.cpp @@ -42,7 +42,16 @@ int main(int argc, char** argv) { graph_builder.dynamicsFactorGraph(robot, 0); // Inverse dynamics priors. We care about the torques. + gtsam::Vector joint_angles = gtsam::Vector::Zero(robot.numJoints()); + gtsam::Vector joint_vels = gtsam::Vector::Zero(robot.numJoints()); gtsam::Vector joint_accels = gtsam::Vector::Zero(robot.numJoints()); + + for (size_t j = 0; j < robot.numJoints(); ++j) { + InsertJointAngle(&joint_angles_vels_accels, j, 0, joint_angles[j]); + InsertJointVel(&joint_angles_vels_accels, j, 0, joint_vels[j]); + InsertJointAccel(&joint_angles_vels_accels, j, 0, joint_accels[j]); + } + gtsam::NonlinearFactorGraph prior_factors = graph_builder.inverseDynamicsPriors(robot, 0, joint_angles_vels_accels); @@ -52,8 +61,7 @@ int main(int argc, char** argv) { prior_factors.addPrior(PoseKey(i, 0), link->bMcom(), gtsam::noiseModel::Constrained::All(6)); prior_factors.addPrior( - 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); diff --git a/scripts/gerry00_ForwardDynamicsPrismatic.cpp b/scripts/gerry00_ForwardDynamicsPrismatic.cpp index 9b2120708..7a6eec7e8 100644 --- a/scripts/gerry00_ForwardDynamicsPrismatic.cpp +++ b/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 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); diff --git a/scripts/nithya00_constrained_opt_benchmark.cpp b/scripts/nithya00_constrained_opt_benchmark.cpp index 11b86afe0..c8b61e74f 100644 --- a/scripts/nithya00_constrained_opt_benchmark.cpp +++ b/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++) { diff --git a/scripts/stephanie01_ForwardDynamicsThreeLink.cpp b/scripts/stephanie01_ForwardDynamicsThreeLink.cpp index 66223c34d..f790fabca 100644 --- a/scripts/stephanie01_ForwardDynamicsThreeLink.cpp +++ b/scripts/stephanie01_ForwardDynamicsThreeLink.cpp @@ -33,6 +33,11 @@ int main(int argc, char **argv) { // Add forward dynamics priors to factor graph. gtsam::Values values; + for (size_t j = 0; j < robot.numJoints(); ++j) { + InsertJointAngle(&values, j, 0, 0.0); + InsertJointVel(&values, j, 0, 0.0); + InsertTorque(&values, j, 0, 0.0); + } auto priorFactors = graph_builder.forwardDynamicsPriors(robot, 0, values); graph.add(priorFactors); From c665e3bd9575cbc11efd5402db7cc61e6ed3a312 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Oct 2023 09:39:44 -0400 Subject: [PATCH 2/2] use initializer to initialize joint values --- gtdynamics/utils/Initializer.cpp | 2 ++ scripts/alejandro_yetong_01_id_four_bar.cpp | 14 ++------------ scripts/stephanie01_ForwardDynamicsThreeLink.cpp | 11 +++-------- 3 files changed, 7 insertions(+), 20 deletions(-) diff --git a/gtdynamics/utils/Initializer.cpp b/gtdynamics/utils/Initializer.cpp index 2192d9796..d21a94868 100644 --- a/gtdynamics/utils/Initializer.cpp +++ b/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 keys = {TorqueKey(j, t), JointAngleKey(j, t), JointVelKey(j, t), JointAccelKey(j, t)}; for (size_t i = 0; i < keys.size(); i++) diff --git a/scripts/alejandro_yetong_01_id_four_bar.cpp b/scripts/alejandro_yetong_01_id_four_bar.cpp index df20fbbcd..226536375 100644 --- a/scripts/alejandro_yetong_01_id_four_bar.cpp +++ b/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,16 +42,7 @@ int main(int argc, char** argv) { graph_builder.dynamicsFactorGraph(robot, 0); // Inverse dynamics priors. We care about the torques. - gtsam::Vector joint_angles = gtsam::Vector::Zero(robot.numJoints()); - gtsam::Vector joint_vels = gtsam::Vector::Zero(robot.numJoints()); - gtsam::Vector joint_accels = gtsam::Vector::Zero(robot.numJoints()); - - for (size_t j = 0; j < robot.numJoints(); ++j) { - InsertJointAngle(&joint_angles_vels_accels, j, 0, joint_angles[j]); - InsertJointVel(&joint_angles_vels_accels, j, 0, joint_vels[j]); - InsertJointAccel(&joint_angles_vels_accels, j, 0, joint_accels[j]); - } - + gtsam::Values joint_angles_vels_accels = initializer.ZeroValues(robot, 0); gtsam::NonlinearFactorGraph prior_factors = graph_builder.inverseDynamicsPriors(robot, 0, joint_angles_vels_accels); @@ -72,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; diff --git a/scripts/stephanie01_ForwardDynamicsThreeLink.cpp b/scripts/stephanie01_ForwardDynamicsThreeLink.cpp index f790fabca..870fe1622 100644 --- a/scripts/stephanie01_ForwardDynamicsThreeLink.cpp +++ b/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,18 +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; - for (size_t j = 0; j < robot.numJoints(); ++j) { - InsertJointAngle(&values, j, 0, 0.0); - InsertJointVel(&values, j, 0, 0.0); - InsertTorque(&values, j, 0, 0.0); - } - + 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.