Merge pull request #5 from ansue1234/vari_fric

Added variable mu per limb

.gitignore

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

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
+}

examples/friction_case/frictionExample.cpp

@@ -30,7 +30,8 @@ void get_robot_description(int argc, char** argv,
     double young_mod = 1e5;
     double density = 509.2985;
     double poisson = 0.5;
-    soft_robots->addLimb(Vector3d(0.0, 0.00, 0.025), Vector3d(1.0, 0.00, 0.025), n, density, radius, young_mod, poisson);
+    double mu = 0.4;
+    soft_robots->addLimb(Vector3d(0.0, 0.00, 0.025), Vector3d(1.0, 0.00, 0.025), n, density, radius, young_mod, poisson, mu);
 
     // Add gravity
     Vector3d gravity_vec(0.0, 0.0, -9.8);
@@ -39,9 +40,8 @@ void get_robot_description(int argc, char** argv,
     // Add floor contact
     double delta = 5e-4;
     double nu = 1e-3;
-    double mu = 0.4;
     double floor_z = 0.0;
-    forces->addForce(make_shared<floorContactForce>(soft_robots, delta, nu, mu, floor_z));
+    forces->addForce(make_shared<floorContactForce>(soft_robots, delta, nu, floor_z));
 
     // Create an external constant uniform force
     shared_ptr<uniformConstantForce> uniform_force = make_shared<uniformConstantForce>(soft_robots);

examples/horton_case/hortonExample.cpp

@@ -27,13 +27,14 @@ void get_robot_description(int argc, char** argv,
     double young_mod = 5e5;
     double density = 1180;
     double poisson = 0.5;
+    double mu = 0.4;
 
     // Create the limbs
-    soft_robots->addLimb(Vector3d(-0.10, 0.00, 0.00), Vector3d(-0.10, 0.00, 0.10), n, density, radius, young_mod, poisson);
-    soft_robots->addLimb(Vector3d(0.00, 0.00, 0.00), Vector3d(0.00, 0.00, 0.10), n, density, radius, young_mod, poisson);
-    soft_robots->addLimb(Vector3d(0.10, 0.00, 0.00), Vector3d(0.10, 0.00, 0.10), n, density, radius, young_mod, poisson);
-    soft_robots->addLimb(Vector3d(-0.10, 0.00, 0.10), Vector3d(0.00, 0.00, 0.10), n, density, radius, young_mod, poisson);
-    soft_robots->addLimb(Vector3d(0.10, 0.00, 0.10), Vector3d(0.00, 0.00, 0.10), n, density, radius, young_mod, poisson);
+    soft_robots->addLimb(Vector3d(-0.10, 0.00, 0.00), Vector3d(-0.10, 0.00, 0.10), n, density, radius, young_mod, poisson, mu);
+    soft_robots->addLimb(Vector3d(0.00, 0.00, 0.00), Vector3d(0.00, 0.00, 0.10), n, density, radius, young_mod, poisson, mu);
+    soft_robots->addLimb(Vector3d(0.10, 0.00, 0.00), Vector3d(0.10, 0.00, 0.10), n, density, radius, young_mod, poisson, mu);
+    soft_robots->addLimb(Vector3d(-0.10, 0.00, 0.10), Vector3d(0.00, 0.00, 0.10), n, density, radius, young_mod, poisson, mu);
+    soft_robots->addLimb(Vector3d(0.10, 0.00, 0.10), Vector3d(0.00, 0.00, 0.10), n, density, radius, young_mod, poisson, mu);
 
     // Create joints and connect appropriately
     soft_robots->createJoint(0, -1);
@@ -54,9 +55,8 @@ void get_robot_description(int argc, char** argv,
     // Add floor contact
     double delta = 5e-4;
     double nu = 5e-3;
-    double mu = 0.4;
     double floor_z = -0.015;
-    forces->addForce(make_shared<floorContactForce>(soft_robots, delta, nu, mu, floor_z));
+    forces->addForce(make_shared<floorContactForce>(soft_robots, delta, nu, floor_z));
 
     // Add kappa_bar controller
     string phi_ctrl_file_path = "src/controllers/openloop_control_trajectories/horton_controller.csv";

examples/spider_case/spiderExample.cpp

@@ -27,17 +27,17 @@ void get_robot_description(int argc, char** argv,
     double young_mod = 3e6;
     double density = 1180;
     double poisson = 0.5;
-
+    double mu = 0.4;
     // Create the limbs
-    soft_robots->addLimb(Vector3d(0, 0, 0.20), Vector3d(0, 0.00, 0.10), n, density, radius, young_mod, poisson);
-    soft_robots->addLimb(Vector3d(0, 0, 0.10), Vector3d(0.10, 0, 0.10), n, density, radius, young_mod, poisson);
-    soft_robots->addLimb(Vector3d(0, 0, 0.10), Vector3d(0, 0.10, 0.10), n, density, radius, young_mod, poisson);
-    soft_robots->addLimb(Vector3d(0, 0, 0.10), Vector3d(0, -0.10, 0.10), n, density, radius, young_mod, poisson);
-    soft_robots->addLimb(Vector3d(0, 0, 0.10), Vector3d(-0.10, 0, 0.10), n, density, radius, young_mod, poisson);
-    soft_robots->addLimb(Vector3d(0.10, 0, 0.10), Vector3d(0.10, 0, 0.00), n, density, radius, young_mod, poisson);
-    soft_robots->addLimb(Vector3d(0.0, 0.10, 0.10), Vector3d(0.0, 0.10, 0.00), n, density, radius, young_mod, poisson);
-    soft_robots->addLimb(Vector3d(0.0, -0.10, 0.10), Vector3d(0.0, -0.10, 0.00), n, density, radius, young_mod, poisson);
-    soft_robots->addLimb(Vector3d(-0.10, 0, 0.10), Vector3d(-0.10, 0, 0.00), n, density, radius, young_mod, poisson);
+    soft_robots->addLimb(Vector3d(0, 0, 0.20), Vector3d(0, 0.00, 0.10), n, density, radius, young_mod, poisson, mu);
+    soft_robots->addLimb(Vector3d(0, 0, 0.10), Vector3d(0.10, 0, 0.10), n, density, radius, young_mod, poisson, mu);
+    soft_robots->addLimb(Vector3d(0, 0, 0.10), Vector3d(0, 0.10, 0.10), n, density, radius, young_mod, poisson, mu);
+    soft_robots->addLimb(Vector3d(0, 0, 0.10), Vector3d(0, -0.10, 0.10), n, density, radius, young_mod, poisson, mu);
+    soft_robots->addLimb(Vector3d(0, 0, 0.10), Vector3d(-0.10, 0, 0.10), n, density, radius, young_mod, poisson, mu);
+    soft_robots->addLimb(Vector3d(0.10, 0, 0.10), Vector3d(0.10, 0, 0.00), n, density, radius, young_mod, poisson, mu);
+    soft_robots->addLimb(Vector3d(0.0, 0.10, 0.10), Vector3d(0.0, 0.10, 0.00), n, density, radius, young_mod, poisson, mu);
+    soft_robots->addLimb(Vector3d(0.0, -0.10, 0.10), Vector3d(0.0, -0.10, 0.00), n, density, radius, young_mod, poisson, mu);
+    soft_robots->addLimb(Vector3d(-0.10, 0, 0.10), Vector3d(-0.10, 0, 0.00), n, density, radius, young_mod, poisson, mu);
 
     // Create joints and connect appropriately
     soft_robots->createJoint(0, -1);
@@ -64,9 +64,8 @@ void get_robot_description(int argc, char** argv,
     // Add floor contact
     double delta = 5e-4;
     double nu = 5e-3;
-    double mu = 0.4;
     double floor_z = -0.10;
-    forces->addForce(make_shared<floorContactForce>(soft_robots, delta, nu, mu, floor_z));
+    forces->addForce(make_shared<floorContactForce>(soft_robots, delta, nu, floor_z));
 
     string logfile_base = "log_files/spider";
     int logging_period = 20;

run_robot.sh

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

src/rod_mechanics/elasticRod.cpp

@@ -1,9 +1,9 @@
 #include "elasticRod.h"
 
 elasticRod::elasticRod(int limb_idx, const Vector3d& start, const Vector3d& end, int num_nodes,
-                       double rho, double rod_radius, double youngs_modulus, double poisson_ratio) :
+                       double rho, double rod_radius, double youngs_modulus, double poisson_ratio, double mu) :
                        limb_idx(limb_idx), ndof(num_nodes*4-1), nv(num_nodes), ne(num_nodes-1), rho(rho),
-                       rod_radius(rod_radius), youngM(youngs_modulus), poisson_ratio(poisson_ratio)
+                       rod_radius(rod_radius), youngM(youngs_modulus), poisson_ratio(poisson_ratio), mu(mu)
 {
     Vector3d dir = (end - start) / (num_nodes - 1);
     for (int i = 0; i < num_nodes; i++)
@@ -18,10 +18,10 @@ elasticRod::elasticRod(int limb_idx, const Vector3d& start, const Vector3d& end,
 
 
 elasticRod::elasticRod(int limb_idx, const vector<Vector3d>& nodes, double rho, double rod_radius,
-                       double youngs_modulus, double poisson_ratio) :
+                       double youngs_modulus, double poisson_ratio, double mu) :
                        limb_idx(limb_idx), ndof(nodes.size()*4-1), nv(nodes.size()), ne(nodes.size()-1),
                        all_nodes(nodes), rho(rho), rod_radius(rod_radius),
-                       youngM(youngs_modulus), poisson_ratio(poisson_ratio)
+                       youngM(youngs_modulus), poisson_ratio(poisson_ratio), mu(mu)
 {
     rod_length = 0;
     for (int i = 1; i < nv; i++)  {

src/rod_mechanics/elasticRod.h

@@ -8,9 +8,9 @@ class elasticRod
     // NOTE: probably could move more stuff to private
     public:
     elasticRod(int limb_idx, const Vector3d& start, const Vector3d& end, int num_nodes,
-               double rho, double rod_radius, double youngs_modulus, double poisson_ratio);
+               double rho, double rod_radius, double youngs_modulus, double poisson_ratio, double mu);
     elasticRod(int limb_idx, const vector<Vector3d>& nodes, double rho, double rod_radius,
-               double youngs_modulus, double poisson_ratio);
+               double youngs_modulus, double poisson_ratio, double mu);
     ~elasticRod();
 
     void setVertexBoundaryCondition(Vector3d position, int k);
@@ -45,7 +45,8 @@ class elasticRod
     double rod_radius;           // cross-sectional radius of the rod
     double cross_sectional_area; // cross-sectional area of the rod
     double dm; // mass per segment
-
+    double mu; // friction coefficient
+
     // Total length
     double rod_length;
 

src/rod_mechanics/external_forces/contactForce.cpp

@@ -1,10 +1,11 @@
 #include "contactForce.h"
 #include "time_steppers/baseTimeStepper.h"
 
+
 contactForce::contactForce(const shared_ptr<softRobots>& soft_robots, double col_limit,
-                           double delta, double k_scaler, double mu, double nu, bool self_contact) :
-                           baseForce(soft_robots), delta(delta), k_scaler(k_scaler),
-                           mu(mu), nu(nu), friction(mu > 0.0)
+                           double delta, double k_scaler, bool friction, double nu, bool self_contact) :
+                           baseForce(soft_robots), delta(delta), scaler(m_k_scaler),
+                           nu(nu), friction(friction)
                            {
 
     col_detector = make_unique<collisionDetector>(soft_robots, col_limit, delta, self_contact);
@@ -17,7 +18,7 @@ contactForce::contactForce(const shared_ptr<softRobots>& soft_robots, double col
     e2p_input[9] = K1;
     e2e_input[12] = K1;
 
-    friction_input[36] = mu;
+    // friction_input[36] = mu;
     friction_input[38] = K2;
 
     sym_eqs = make_unique<symbolicEquations>();
@@ -104,7 +105,8 @@ void contactForce::setupContactVariables(const Vector<int, 8>& contact_id) {
     auto limb2 = soft_robots->limbs[idx6];
 
     surface_limit = limb1->rod_radius + limb2->rod_radius;
-
+    // Let mu be the max of the two mu's of the two limbs
+    mu = max(limb1->mu, limb2->mu); 
     x1s = limb1->getVertex(idx1);
     x1e = limb1->getVertex(idx3);
     x2s = limb2->getVertex(idx2);
@@ -140,7 +142,7 @@ void contactForce::prepContactInput() {
     }
 }
 
-
+// TODO change friction coefficent here
 void contactForce::prepFrictionInput(double dt) {
     auto limb1 = soft_robots->limbs[idx5];
     auto limb2 = soft_robots->limbs[idx6];
@@ -154,6 +156,7 @@ void contactForce::prepFrictionInput(double dt) {
     friction_input.segment<3>(18) = x2s0;
     friction_input.segment<3>(21) = x2e0;
     friction_input.segment<12>(24) = contact_gradient;
+    friction_input[36] = mu;
     friction_input[37] = dt;
 }
 

src/rod_mechanics/external_forces/contactForce.h

@@ -13,7 +13,8 @@ class contactForce : public baseForce
 {
 public:
     contactForce(const shared_ptr<softRobots>& soft_robots,
-                 double col_limit, double delta, double k_scaler, double mu, double nu, bool self_contact);
+                 double col_limit, double delta, double k_scaler, bool friction, double nu, bool self_contact);
+
 
 //    void updateContactStiffness();
     void computeForce(double dt) override;

src/rod_mechanics/external_forces/floorContactForce.cpp

@@ -7,11 +7,11 @@
  * TODO: ADD MORE EFFICIENT COLLISION DETECTION
  */
 
-
+// Floor friction defaults to the friction coefficient of the limb if floor mu is not specified
 floorContactForce::floorContactForce(const shared_ptr<softRobots>& soft_robots, double floor_delta,
-                                     double floor_slipTol, double floor_mu, double floor_z) :
+                                     double floor_slipTol, double floor_z, double floor_mu) :
                                      baseForce(soft_robots), delta(floor_delta), slipTol(floor_slipTol),
-                                     mu(floor_mu), floor_z(floor_z)
+                                     floor_z(floor_z), floor_mu(floor_mu)
 {
     K1 = 15 / delta;
     K2 = 15 / slipTol;
@@ -22,7 +22,6 @@ floorContactForce::floorContactForce(const shared_ptr<softRobots>& soft_robots,
 
     contact_input[1] = K1;
 
-    fric_jacobian_input[5] = mu;
     fric_jacobian_input[7] = K2;
 
     sym_eqs = make_shared<symbolicEquations>();
@@ -46,12 +45,6 @@ void floorContactForce::change_slip_tol(double scale) {
 }
 
 
-void floorContactForce::updateMu(double m_mu) {
-    mu = m_mu;
-    fric_jacobian_input[5] = mu;
-}
-
-
 void floorContactForce::computeForce(double dt) {
     double dist;
     double f;
@@ -77,10 +70,11 @@ void floorContactForce::computeForce(double dt) {
 
             stepper->addForce(ind, f, limb_idx);
 
-            // Apply friction force
+            // Apply friction force, get mu from limb
+            double mu = (floor_mu < 0) ? limb->mu : floor_mu;
             curr_node = limb->getVertex(i)(seq(0, 1));
             pre_node = limb->getPreVertex(i)(seq(0, 1));
-            computeFriction(curr_node, pre_node, f, dt);
+            computeFriction(curr_node, pre_node, f, mu, dt);
             stepper->addForce(4*i, ffr(0), limb_idx);
             stepper->addForce(4*i+1, ffr(1), limb_idx);
 
@@ -121,18 +115,19 @@ void floorContactForce::computeForceAndJacobian(double dt) {
             stepper->addForce(ind, f, limb_idx);
             stepper->addJacobian(ind, ind, J, limb_idx);
 
-            // Friction forces
+            // Friction forces, get mu from limb
+            double mu = (floor_mu < 0) ? limb->mu : floor_mu;
             curr_node = limb->getVertex(i)(seq(0, 1));
             pre_node = limb->getPreVertex(i)(seq(0, 1));
-            computeFriction(curr_node, pre_node, f, dt);
+            computeFriction(curr_node, pre_node, f, mu, dt);
 
             if (fric_jaco_type == 0) continue;
 
             stepper->addForce(4*i, ffr(0), limb_idx);
             stepper->addForce(4*i+1, ffr(1), limb_idx);
 
             // Friction jacobian
-            prepFrictionJacobianInput(curr_node, pre_node, f,  dt);
+            prepFrictionJacobianInput(curr_node, pre_node, f, mu, dt);
 
             if (fric_jaco_type == 1) {
                 sym_eqs->floor_friction_partials_gamma1_dfr_dx_func.call(friction_partials_dfr_dx.data(), fric_jacobian_input.data());
@@ -161,7 +156,7 @@ void floorContactForce::computeForceAndJacobian(double dt) {
 }
 
 
-void floorContactForce::computeFriction(const Vector2d& curr_node, const Vector2d& pre_node, double fn, double dt) {
+void floorContactForce::computeFriction(const Vector2d& curr_node, const Vector2d& pre_node, double fn, double mu, double dt) {
     Vector2d v, v_hat;
     double v_n, gamma;
 
@@ -188,11 +183,12 @@ void floorContactForce::computeFriction(const Vector2d& curr_node, const Vector2
 
 
 void floorContactForce::prepFrictionJacobianInput(const Vector2d& curr_node, const Vector2d& pre_node,
-                                                  double fn, double dt) {
+                                                  double fn, double mu, double dt) {
     fric_jacobian_input(0) = curr_node(0);
     fric_jacobian_input(1) = curr_node(1);
     fric_jacobian_input(2) = pre_node(0);
     fric_jacobian_input(3) = pre_node(1);
     fric_jacobian_input(4) = fn;
+    fric_jacobian_input[5] = mu;
     fric_jacobian_input[6] = dt;
 }

src/rod_mechanics/external_forces/floorContactForce.h

@@ -10,12 +10,11 @@ class floorContactForce : public baseForce
 {
 public:
     floorContactForce(const shared_ptr<softRobots>& soft_robots, double floor_delta,
-                      double floor_slipTol, double mu, double floor_z);
+                      double floor_slipTol, double floor_z, double floor_mu=-1.0);
     ~floorContactForce() override;
 
     void computeForce(double dt) override;
     void computeForceAndJacobian(double dt) override;
-    void updateMu(double mu);
 
     double min_dist;
     int num_contacts;
@@ -24,8 +23,8 @@ class floorContactForce : public baseForce
     void reset_slip_tol();
 
 private:
-    void computeFriction(const Vector2d& curr_node, const Vector2d& pre_node, double fn, double dt);
-    void prepFrictionJacobianInput(const Vector2d& curr_node, const Vector2d& pre_node, double fn, double dt);
+    void computeFriction(const Vector2d& curr_node, const Vector2d& pre_node, double fn, double mu, double dt);
+    void prepFrictionJacobianInput(const Vector2d& curr_node, const Vector2d& pre_node, double fn, double mu, double dt);
 
     shared_ptr<symbolicEquations> sym_eqs;
     Vector<double, 2> contact_input;
@@ -35,7 +34,7 @@ class floorContactForce : public baseForce
     Vector<double, 2> ffr;
     int fric_jaco_type;
     double contact_stiffness;
-    double mu;
+    double floor_mu;
     double delta;
     double slipTol;
     double orig_slip_tol;