Added node specific constant force

.gitignore

@@ -4,6 +4,11 @@ build/
 cmake-build-debug/
 
 disMech
+simDER
+
+run_SML_robot.sh
+custom_bot/SML_Robots/
+
 datafiles/
 option.txt
 robotDescription.cpp

CMakeLists.txt

@@ -71,6 +71,7 @@ add_executable(disMech
         src/rod_mechanics/external_forces/floorContactForce.cpp
         src/rod_mechanics/external_forces/symbolicEquations.cpp
         src/rod_mechanics/external_forces/uniformConstantForce.cpp
+        src/rod_mechanics/external_forces/nonUniformConstantForce.cpp
         src/rod_mechanics/external_forces/collisionDetector.cpp
         src/rod_mechanics/external_forces/contactForce.cpp
 

compile_build.sh

@@ -0,0 +1,15 @@
+#!/usr/bin/env bash
+if [ -d "build" ]; then
+  echo "Deleting previous build..."
+  rm -rf build
+fi
+
+echo "Creating new build..."
+cp custom_bot/customBot.cpp robotDescription.cpp
+mkdir build 
+cd build
+cmake ..
+make -j4
+cd ..
+
+echo "Build complete."

custom_bot/customBot.cpp

@@ -0,0 +1,18 @@
+#include "robotDescription.h"
+#include <cmath>
+
+extern ofstream logging_output_file;  // defined in main.cpp
+/*
+ *
+ * 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) {
+
+    //  YOUR ROBOT HERE
+}

run_example.sh

@@ -0,0 +1,7 @@
+#!/usr/bin/env bash
+cp examples/$1_case/$1Example.cpp robotDescription.cpp
+cd build
+make -j4
+cd ..
+
+./dismech.sh $2

run_robot.sh

@@ -0,0 +1,7 @@
+#!/usr/bin/env bash
+cp custom_bot/customBot.cpp robotDescription.cpp
+cd build
+make -j4
+cd ..
+
+./dismech.sh

src/robotDescription.h

@@ -11,6 +11,7 @@
 #include "rod_mechanics/external_forces/gravityForce.h"
 #include "rod_mechanics/external_forces/floorContactForce.h"
 #include "rod_mechanics/external_forces/uniformConstantForce.h"
+#include "rod_mechanics/external_forces/nonUniformConstantForce.h"
 #include "rod_mechanics/external_forces/contactForce.h"
 
 #include "utils/utils.h"

src/rod_mechanics/external_forces/nonUniformConstantForce.cpp

@@ -0,0 +1,56 @@
+#include "nonUniformConstantForce.h"
+#include "time_steppers/baseTimeStepper.h"
+#include <iostream>
+
+// Applies a constant force to a specific node of a specific LIMB
+// to limbs only, not joints
+nonUniformConstantForce::nonUniformConstantForce(const shared_ptr<softRobots>& m_soft_robots) :
+                                           baseForce(m_soft_robots)
+{
+}
+
+nonUniformConstantForce::~nonUniformConstantForce() = default;
+
+void nonUniformConstantForce::add_force_to_limb_node(int limb_idx, int node_idx, const Vector3d& force) {
+    struct limbNodeForceItem force_item;
+    force_item.limb_idx = limb_idx;
+    force_item.node_idx = node_idx;
+    force_item.force = force;
+    limb_node_forces.emplace_back(force_item);
+}
+
+void nonUniformConstantForce::computeForce(double dt) {
+    int limb_idx;
+    int node_idx;
+    shared_ptr<elasticRod> limb;
+    Vector3d force;
+    for (const auto& force_item : limb_node_forces) {
+        limb_idx = force_item.limb_idx;
+        node_idx = force_item.node_idx;
+
+        if (limb_idx >= soft_robots->limbs.size()) {
+            std::cout << "ERROR: limb index out of range" << std::endl;
+            exit(1);
+        }
+        limb = soft_robots->limbs[limb_idx];
+
+        force = force_item.force;
+
+        if (node_idx == -1) {
+            node_idx = limb->nv-1;
+        }
+
+        if (node_idx >= limb->nv) {
+            std::cout << "ERROR: node index out of range" << std::endl;
+            exit(1);
+        }
+        stepper->addForce(4*node_idx, force(0), limb_idx);
+        stepper->addForce(4*node_idx+1, force(1), limb_idx);
+        stepper->addForce(4*node_idx+2, force(2), limb_idx);
+
+    }
+}
+
+void nonUniformConstantForce::computeForceAndJacobian(double dt) {
+    computeForce(dt);
+}

src/rod_mechanics/external_forces/nonUniformConstantForce.h

@@ -0,0 +1,30 @@
+#ifndef NON_UNIFORM_CONSTANT_FORCE_H
+#define NON_UNIFORM_CONSTANT_FORCE_H
+
+#include "rod_mechanics/baseForce.h"
+
+class baseTimeStepper;
+
+// declares a structure to hold a force on a specific node of a specific limb
+struct limbNodeForceItem{            
+    int limb_idx;         
+    int node_idx;
+    Vector3d force;
+};  
+
+class nonUniformConstantForce : public baseForce
+{
+public:
+    nonUniformConstantForce(const shared_ptr<softRobots>& m_soft_robots);
+    ~nonUniformConstantForce() override;
+
+    void add_force_to_limb_node(int limb_idx, int node_idx, const Vector3d& force);
+
+    void computeForce(double dt) override;
+    void computeForceAndJacobian(double dt) override;
+
+private:
+    vector<limbNodeForceItem> limb_node_forces;
+};
+
+#endif

src/rod_mechanics/softRobots.cpp

@@ -46,7 +46,6 @@ void softRobots::lockEdge(int limb_idx, int edge_idx) {
     limb->setThetaBoundaryCondition(0.0, edge_idx);
 }
 
-
 void softRobots::applyInitialVelocities(int limb_idx, const vector<Vector3d> &velocities) {
     shared_ptr<elasticRod> limb = limbs[limb_idx];
     if (limb->nv != velocities.size()) {