Merge branch 'main' into vari_fric

CMakeLists.txt

@@ -96,8 +96,9 @@ add_executable(disMech
         src/simulation_environments/headlessDERSimulationEnvironment.cpp
         src/simulation_environments/openglDERSimulationEnvironment.cpp
 
-        src/controllers/rodController.cpp
-        src/controllers/rodOpenLoopFileKappabarSetter.cpp
+        src/controllers/baseController.cpp
+        src/controllers/openLoopUniformKappaBarController.cpp
+        src/controllers/activeEntanglementController.cpp
 
         src/utils/utils.cpp
         )

examples/active_entanglement_case/activeEntanglementExample.cpp

@@ -0,0 +1,74 @@
+#include "robotDescription.h"
+#include <math.h>
+
+
+extern ofstream logging_output_file;  // defined in main.cpp
+/*
+ * Dynamic Cantilever Example
+ *
+ * Define your soft robot structure(s), boundary conditions,
+ * custom external forces, and loggers in the function below.
+ */
+
+void get_robot_description(int argc, char** argv,
+                           const shared_ptr<softRobots>& soft_robots,
+                           const shared_ptr<forceContainer>& forces,
+                           shared_ptr<worldLogger>& logger,
+                           simParams& sim_params) {
+
+    sim_params.dt = 2.5e-3;
+    sim_params.sim_time = 15;
+    sim_params.dtol = 1e-2;
+    sim_params.render_scale = 3.0;
+    sim_params.adaptive_time_stepping = 10;
+
+    int n = 60;
+    double radius = 0.005;
+    double young_mod = 3e5;
+    double density = 1200;
+    double poisson = 0.5;
+
+    int num_fingers = 5;
+    double dist = 0.015;
+
+    double theta = (num_fingers - 2) * M_PI / (2 * num_fingers);
+    double R = dist / 2.0 / cos(theta);
+    theta = 2 * M_PI / num_fingers;
+
+    for (int i = 0; i < num_fingers; i++) {
+
+        double t = theta * i;
+        double x = R - R * cos(t);
+        double y = R * sin(t);
+
+
+        soft_robots->addLimb(Vector3d(x, y, 0.0), Vector3d(x, y, -0.3), n, density, radius, young_mod, poisson);
+        soft_robots->lockEdge(i, 0);
+    }
+
+    // Add gravity
+    Vector3d gravity_vec(0.0, 0.0, -9.8);
+    forces->addForce(make_shared<gravityForce>(soft_robots, gravity_vec));
+
+    // Add viscous damping
+    double viscosity = 5.0;
+    forces->addForce(make_shared<dampingForce>(soft_robots, viscosity));
+
+    // Add self-contact
+    double col_limit = 1e-3;
+    double delta = 5e-4;
+    double k_scaler = 1e5;
+    double mu = 0.4;
+    double nu = 1e-3;
+    bool self_contact = true;
+    forces->addForce(make_shared<contactForce>(soft_robots, col_limit, delta, k_scaler, mu, nu, self_contact));
+
+    // Add custom active entanglement controller
+    double start_time = 0.0;
+    double end_time = 10.0;
+    soft_robots->addController(make_shared<activeEntanglementController>(soft_robots, start_time, end_time));
+
+//    string logfile_base = "log_files/active_entanglement";
+//    int logging_period = 5;
+//    logger = make_shared<rodNodeLogger>(logfile_base, logging_output_file, logging_period);
+}

examples/horton_case/hortonExample.cpp

@@ -21,8 +21,7 @@ void get_robot_description(int argc, char** argv,
     sim_params.show_mat_frames = true;
     sim_params.enable_2d_sim = true;
     sim_params.adaptive_time_stepping = 20;
-    sim_params.nis = IMPLICIT_MIDPOINT;
-
+
     int n = 15;
     double radius = 0.015;
     double young_mod = 5e5;
@@ -61,5 +60,5 @@ void get_robot_description(int argc, char** argv,
 
     // Add kappa_bar controller
     string phi_ctrl_file_path = "src/controllers/openloop_control_trajectories/horton_controller.csv";
-    soft_robots->addController(make_shared<rodOpenLoopFileKappabarSetter>(soft_robots, phi_ctrl_file_path));
+    soft_robots->addController(make_shared<openLoopUniformKappaBarController>(soft_robots, phi_ctrl_file_path));
 }

src/controllers/activeEntanglementController.cpp

@@ -0,0 +1,42 @@
+#include "activeEntanglementController.h"
+#include "rod_mechanics/softRobots.h"
+
+
+activeEntanglementController::activeEntanglementController(const shared_ptr <softRobots> &soft_robots,
+                                                           double start_time, double end_time) :
+                                                           baseController(soft_robots->limbs),
+                                                           start_time(start_time), end_time(end_time)
+{
+    for (int limb_idx = 0; limb_idx < num_actuators; limb_idx++) {
+        auto limb = limbs[limb_idx];
+
+        random_curvatures.emplace_back();
+
+        for (int i = 1; i < limb->ne; i++) {
+            double random_curvature = rand() % 40 * (M_PI / 180);
+            random_curvatures[limb_idx].push_back(random_curvature);
+        }
+    }
+}
+
+
+activeEntanglementController::~activeEntanglementController() = default;
+
+
+void activeEntanglementController::updateTimeStep(double dt) {
+    baseController::updateTimeStep(dt);
+
+    if (current_time < start_time || current_time > end_time) return;
+
+    double curr_ratio = 0.5 * (current_time - start_time) / (end_time - start_time);
+
+    for (int limb_idx = 0; limb_idx < num_actuators; limb_idx++) {
+        auto limb = limbs[limb_idx];
+
+        for (int i = 1; i < limb->ne; i++) {
+            double random_curvature = 2 * tan(random_curvatures.at(limb_idx).at(i-1) * curr_ratio * 0.5);
+            limb->kappa_bar(i, 0) = random_curvature;
+            limb->kappa_bar(i, 1) = random_curvature;
+        }
+    }
+}

src/controllers/activeEntanglementController.h

@@ -0,0 +1,22 @@
+#ifndef ACTIVE_ENTANGLEMENT_CONTROLLER_H
+#define ACTIVE_ENTANGLEMENT_CONTROLLER_H
+
+#include "baseController.h"
+
+class softRobots;
+
+class activeEntanglementController : public baseController
+{
+public:
+    explicit activeEntanglementController(const shared_ptr<softRobots>& soft_robots, double start_time, double end_time);
+    ~activeEntanglementController();
+
+    void updateTimeStep(double dt) override;
+
+private:
+    vector<vector<double>> random_curvatures;
+    double start_time;
+    double end_time;
+};
+
+#endif

src/controllers/rodController.cpp → src/controllers/baseController.cpp

@@ -1,17 +1,17 @@
-#include "rodController.h"
+#include "baseController.h"
 
 
-rodController::rodController(const vector<shared_ptr<elasticRod>>& limbs) :
+baseController::baseController(const vector<shared_ptr<elasticRod>>& limbs) :
                              limbs(limbs), num_actuators(limbs.size()), current_time(0)
 {
 }
 
 
-rodController::~rodController() = default;
+baseController::~baseController() = default;
 
 
 // but we can also implement the timestepping here, for others to override if desired.
-void rodController::updateTimestep(double dt)
+void baseController::updateTimeStep(double dt)
 {
     current_time += dt;
 }

src/controllers/baseController.h

@@ -0,0 +1,22 @@
+#ifndef BASE_CONTROLLER_H
+#define BASE_CONTROLLER_H
+
+#include "eigenIncludes.h"
+
+class elasticRod;
+
+class baseController
+{
+public:
+    explicit baseController(const vector<shared_ptr<elasticRod>>& limbs);
+    ~baseController();
+
+    virtual void updateTimeStep(double dt);
+
+protected:
+    vector<shared_ptr<elasticRod>> limbs;
+    int num_actuators;
+    double current_time;
+};
+
+#endif

src/controllers/rodOpenLoopFileKappabarSetter.cpp → src/controllers/openLoopUniformKappaBarController.cpp

@@ -1,10 +1,10 @@
-#include "rodOpenLoopFileKappabarSetter.h"
+#include "openLoopUniformKappaBarController.h"
 #include "rod_mechanics/softRobots.h"
 #include <fstream>
 
 
-rodOpenLoopFileKappabarSetter::rodOpenLoopFileKappabarSetter(const shared_ptr<softRobots>& soft_robots, string filepath) :
-                                                             rodController(soft_robots->limbs)
+openLoopUniformKappaBarController::openLoopUniformKappaBarController(const shared_ptr<softRobots>& soft_robots, string filepath) :
+                                                                     baseController(soft_robots->limbs)
 {
     // to-do: validate number of columns of file vs. numAct.
     // if (numAct != m_periods.size())
@@ -31,10 +31,10 @@ rodOpenLoopFileKappabarSetter::rodOpenLoopFileKappabarSetter(const shared_ptr<so
 }
 
 
-rodOpenLoopFileKappabarSetter::~rodOpenLoopFileKappabarSetter() = default;
+openLoopUniformKappaBarController::~openLoopUniformKappaBarController() = default;
 
 
-void rodOpenLoopFileKappabarSetter::parseActuationFile(string csv_path)
+void openLoopUniformKappaBarController::parseActuationFile(string csv_path)
 {
     // open the file and check if it worked
     ifstream csv_file(csv_path);
@@ -84,10 +84,10 @@ void rodOpenLoopFileKappabarSetter::parseActuationFile(string csv_path)
 }
 
 // override the base class implementation to include PWMs
-void rodOpenLoopFileKappabarSetter::updateTimestep(double dt)
+void openLoopUniformKappaBarController::updateTimeStep(double dt)
 {
     // call the base class to update current_time
-    rodController::updateTimestep(dt);
+    baseController::updateTimeStep(dt);
     // Find the corresponding timepoint in the list.
     // The actuation file is written by hand so it's probably pretty short, iterating through isn't too inefficient
     int idx = 0;
@@ -96,7 +96,7 @@ void rodOpenLoopFileKappabarSetter::updateTimestep(double dt)
     {
         idx++;
     }
-    // the above actually gives us index+1, since we did a bit of manuevering to prevent array out of bounds.
+    // the above actually gives us index+1, since we did a bit of maneuvering to prevent array out of bounds.
     idx--;
     // Timepoint check during the gait: are we at the next row of the actuation file?
     if (idx > prev_time_pt_idx)
@@ -116,14 +116,23 @@ void rodOpenLoopFileKappabarSetter::updateTimestep(double dt)
 }
 
 // Implementation of the controller.
-void rodOpenLoopFileKappabarSetter::updatePhies()
+void openLoopUniformKappaBarController::updatePhies()
 {
     int idx1, idx2;
+    double angle1, angle2;
     // Result should be same length as number of actuators
     for (int limb_idx = 0; limb_idx < num_actuators; limb_idx++)
     {
+        auto limb = limbs[limb_idx];
         idx1 = 2 * limb_idx;
         idx2 = 2 * limb_idx + 1;
-        limbs[limb_idx]->updatePhis(desired_phi_list[idx1], desired_phi_list[idx2]);
+
+        angle1 = desired_phi_list[idx1] / limb->ne * (M_PI / 180);
+        angle2 = desired_phi_list[idx2] / limb->ne * (M_PI / 180);
+
+        for (int i = 1; i < limb->ne; i++) {
+            limb->kappa_bar(i, 0) = 2 * tan(angle1 / 2);
+            limb->kappa_bar(i, 1) = 2 * tan(angle2 / 2);
+        }
     }
 }

src/controllers/rodOpenLoopFileKappabarSetter.h → src/controllers/openLoopUniformKappaBarController.h

@@ -1,19 +1,19 @@
-#ifndef ROD_OPEN_LOOP_FILE_KAPPABAR_SETTER_H
-#define ROD_OPEN_LOOP_FILE_KAPPABAR_SETTER_H
+#ifndef OPEN_LOOP_UNIFORM_KAPPABAR_CONTROLLER_H
+#define OPEN_LOOP_UNIFORM_KAPPABAR_CONTROLLER_H
 
-#include "rodController.h"
+#include "baseController.h"
 
 class softRobots;
 
-class rodOpenLoopFileKappabarSetter : public rodController
+class openLoopUniformKappaBarController : public baseController
 {
 
 public:
-    rodOpenLoopFileKappabarSetter(const shared_ptr<softRobots>& soft_robots, string file_path);
-    ~rodOpenLoopFileKappabarSetter();
+    openLoopUniformKappaBarController(const shared_ptr<softRobots>& soft_robots, string file_path);
+    ~openLoopUniformKappaBarController();
 
     // redefine the base class' timestepping function to also include the PWMs.
-    void updateTimestep(double dt) override;
+    void updateTimeStep(double dt) override;
 
 private:
     // helper to parse the csv file. Writes data into time_pts and dutys.
@@ -26,8 +26,8 @@ class rodOpenLoopFileKappabarSetter : public rodController
     // The CSV file will have a certain number of lines as a header. We'll need to remove them.
     int csv_header_lines = 1;
 
-    // For debugging, if desired: detect when we've change to the next timepoint, so we can pause and inspect the gait.
+    // For debugging, if desired: detect when we've changed to the next timepoint, so we can pause and inspect the gait.
     int prev_time_pt_idx = 0;
 };
 
-#endif // ROD_OPEN_LOOP_FILE_KAPPABAR_SETTER_H
+#endif

src/controllers/openloop_control_trajectories/.~lock.active_entanglement_controller.csv#

@@ -0,0 +1 @@
+,asjchoi,QuantuMopeDT,18.12.2023 13:28,file:///home/asjchoi/.config/libreoffice/4;

src/controllers/openloop_control_trajectories/active_entanglement_controller.csv

@@ -0,0 +1,5 @@
+Simulation time (sec),kappabar_phi_00,kappabar_phi_01,kappabar_phi10,kappabar_phi11,kappabar_phi20,kappabar_phi21,kappabar_phi30,kappabar_phi31,kappabar_phi40,kappabar_phi41,Empty
+0.0,0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,
+0.01,0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,
+5.2,0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,
+5.3,0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,