From 27ff33c30858d2dea0f87e25137c13620c5f7d7f Mon Sep 17 00:00:00 2001 From: Andrew Sue Date: Thu, 2 Nov 2023 17:58:18 -0700 Subject: [PATCH 1/3] Added node specific constant force --- .gitignore | 5 ++ CMakeLists.txt | 1 + compile_build.sh | 15 +++++ custom_bot/customBot.cpp | 18 ++++++ run_example.sh | 7 +++ run_robot.sh | 7 +++ src/robotDescription.h | 1 + .../nonUniformConstantForce.cpp | 56 +++++++++++++++++++ .../external_forces/nonUniformConstantForce.h | 30 ++++++++++ src/rod_mechanics/softRobots.cpp | 1 - 10 files changed, 140 insertions(+), 1 deletion(-) create mode 100644 compile_build.sh create mode 100644 custom_bot/customBot.cpp create mode 100644 run_example.sh create mode 100644 run_robot.sh create mode 100644 src/rod_mechanics/external_forces/nonUniformConstantForce.cpp create mode 100644 src/rod_mechanics/external_forces/nonUniformConstantForce.h diff --git a/.gitignore b/.gitignore index 4cebd082..b43f77f6 100644 --- a/.gitignore +++ b/.gitignore @@ -4,6 +4,11 @@ build/ cmake-build-debug/ disMech +simDER + +run_SML_robot.sh +custom_bot/SML_Robots/ + datafiles/ option.txt robotDescription.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b6033f68..6ac98190 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/compile_build.sh b/compile_build.sh new file mode 100644 index 00000000..d3db91fe --- /dev/null +++ b/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." \ No newline at end of file diff --git a/custom_bot/customBot.cpp b/custom_bot/customBot.cpp new file mode 100644 index 00000000..19913a89 --- /dev/null +++ b/custom_bot/customBot.cpp @@ -0,0 +1,18 @@ +#include "robotDescription.h" +#include + +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& soft_robots, + const shared_ptr& forces, + shared_ptr& logger, + simParams& sim_params) { + + // YOUR ROBOT HERE +} diff --git a/run_example.sh b/run_example.sh new file mode 100644 index 00000000..b0ee1822 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/run_robot.sh b/run_robot.sh new file mode 100644 index 00000000..9900ebbb --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/robotDescription.h b/src/robotDescription.h index 6b9ad260..26f11490 100755 --- a/src/robotDescription.h +++ b/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" diff --git a/src/rod_mechanics/external_forces/nonUniformConstantForce.cpp b/src/rod_mechanics/external_forces/nonUniformConstantForce.cpp new file mode 100644 index 00000000..56edbdc8 --- /dev/null +++ b/src/rod_mechanics/external_forces/nonUniformConstantForce.cpp @@ -0,0 +1,56 @@ +#include "nonUniformConstantForce.h" +#include "time_steppers/baseTimeStepper.h" +#include + +// Applies a constant force to a specific node of a specific LIMB +// to limbs only, not joints +nonUniformConstantForce::nonUniformConstantForce(const shared_ptr& 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 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); +} \ No newline at end of file diff --git a/src/rod_mechanics/external_forces/nonUniformConstantForce.h b/src/rod_mechanics/external_forces/nonUniformConstantForce.h new file mode 100644 index 00000000..9e2ca65a --- /dev/null +++ b/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& 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 limb_node_forces; +}; + +#endif diff --git a/src/rod_mechanics/softRobots.cpp b/src/rod_mechanics/softRobots.cpp index 261f8d1c..6b603d02 100644 --- a/src/rod_mechanics/softRobots.cpp +++ b/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 &velocities) { shared_ptr limb = limbs[limb_idx]; if (limb->nv != velocities.size()) { From ec196f039d0309b14d561e8d333000922edc2cdf Mon Sep 17 00:00:00 2001 From: Andrew Sue Date: Thu, 2 Nov 2023 18:45:27 -0700 Subject: [PATCH 2/3] Trying variable friction --- src/rod_mechanics/elasticRod.cpp | 8 ++++---- src/rod_mechanics/elasticRod.h | 7 ++++--- src/rod_mechanics/external_forces/contactForce.cpp | 2 +- src/rod_mechanics/external_forces/floorContactForce.h | 1 + src/rod_mechanics/softRobots.cpp | 4 ++-- src/rod_mechanics/softRobots.h | 4 ++-- 6 files changed, 14 insertions(+), 12 deletions(-) diff --git a/src/rod_mechanics/elasticRod.cpp b/src/rod_mechanics/elasticRod.cpp index eb1a7bf2..d9ccc88d 100755 --- a/src/rod_mechanics/elasticRod.cpp +++ b/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& 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++) { diff --git a/src/rod_mechanics/elasticRod.h b/src/rod_mechanics/elasticRod.h index 0bd43b1a..e322fd55 100755 --- a/src/rod_mechanics/elasticRod.h +++ b/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& 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); @@ -46,7 +46,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; // Bending curvature angle phi (from the end to the tip of the limb, for each edge: phi_e = phi/ne) diff --git a/src/rod_mechanics/external_forces/contactForce.cpp b/src/rod_mechanics/external_forces/contactForce.cpp index b1411985..8851e701 100755 --- a/src/rod_mechanics/external_forces/contactForce.cpp +++ b/src/rod_mechanics/external_forces/contactForce.cpp @@ -140,7 +140,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]; diff --git a/src/rod_mechanics/external_forces/floorContactForce.h b/src/rod_mechanics/external_forces/floorContactForce.h index 5c698663..a25b2391 100644 --- a/src/rod_mechanics/external_forces/floorContactForce.h +++ b/src/rod_mechanics/external_forces/floorContactForce.h @@ -15,6 +15,7 @@ class floorContactForce : public baseForce void computeForce(double dt) override; void computeForceAndJacobian(double dt) override; + // No need void updateMu(double mu); double min_dist; diff --git a/src/rod_mechanics/softRobots.cpp b/src/rod_mechanics/softRobots.cpp index 6b603d02..36f331cd 100644 --- a/src/rod_mechanics/softRobots.cpp +++ b/src/rod_mechanics/softRobots.cpp @@ -7,7 +7,7 @@ softRobots::~softRobots() = default; void softRobots::addLimb(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) { limbs.push_back(make_shared(num_limbs, start, end, num_nodes, rho, rod_radius, youngs_modulus, poisson_ratio)); num_limbs++; @@ -15,7 +15,7 @@ void softRobots::addLimb(const Vector3d& start, const Vector3d& end, int num_nod void softRobots::addLimb(const vector &nodes, double rho, double rod_radius, double youngs_modulus, - double poisson_ratio) { + double poisson_ratio, double mu) { limbs.push_back(make_shared(num_limbs, nodes, rho, rod_radius, youngs_modulus, poisson_ratio)); num_limbs++; } diff --git a/src/rod_mechanics/softRobots.h b/src/rod_mechanics/softRobots.h index 2a7b8b47..b73980ae 100644 --- a/src/rod_mechanics/softRobots.h +++ b/src/rod_mechanics/softRobots.h @@ -15,9 +15,9 @@ class softRobots ~softRobots(); void addLimb(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); void addLimb(const vector& nodes, double rho, double rod_radius, - double youngs_modulus, double poisson_ratio); + double youngs_modulus, double poisson_ratio, double mu); void createJoint(int limb_idx, int node_idx); void addToJoint(int joint_idx, int limb_idx, int node_idx); From 88dd95aba7f30d7a6ceefa4585f0f0a9d839cc30 Mon Sep 17 00:00:00 2001 From: Andrew Sue Date: Thu, 2 Nov 2023 23:13:48 -0700 Subject: [PATCH 3/3] Added func. for different mu per limb --- .gitignore | 3 + CMakeLists.txt | 1 - compile_build.sh | 15 ----- examples/friction_case/frictionExample.cpp | 6 +- examples/horton_case/hortonExample.cpp | 14 ++--- examples/spider_case/spiderExample.cpp | 23 ++++---- run_example.sh | 7 --- run_robot.sh | 4 +- src/robotDescription.h | 1 - .../external_forces/contactForce.cpp | 11 ++-- .../external_forces/contactForce.h | 2 +- .../external_forces/floorContactForce.cpp | 30 +++++----- .../external_forces/floorContactForce.h | 8 +-- .../nonUniformConstantForce.cpp | 56 ------------------- .../external_forces/nonUniformConstantForce.h | 30 ---------- src/rod_mechanics/softRobots.cpp | 4 +- src/rod_mechanics/softRobots.h | 4 +- 17 files changed, 54 insertions(+), 165 deletions(-) delete mode 100644 compile_build.sh delete mode 100644 run_example.sh delete mode 100644 src/rod_mechanics/external_forces/nonUniformConstantForce.cpp delete mode 100644 src/rod_mechanics/external_forces/nonUniformConstantForce.h diff --git a/.gitignore b/.gitignore index b43f77f6..f33819bd 100644 --- a/.gitignore +++ b/.gitignore @@ -7,6 +7,9 @@ disMech simDER run_SML_robot.sh +run_example.sh +compile_build.sh +run.sh custom_bot/SML_Robots/ datafiles/ diff --git a/CMakeLists.txt b/CMakeLists.txt index 6ac98190..b6033f68 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -71,7 +71,6 @@ 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 diff --git a/compile_build.sh b/compile_build.sh deleted file mode 100644 index d3db91fe..00000000 --- a/compile_build.sh +++ /dev/null @@ -1,15 +0,0 @@ -#!/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." \ No newline at end of file diff --git a/examples/friction_case/frictionExample.cpp b/examples/friction_case/frictionExample.cpp index 763f6fe9..f76fe985 100755 --- a/examples/friction_case/frictionExample.cpp +++ b/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(soft_robots, delta, nu, mu, floor_z)); + forces->addForce(make_shared(soft_robots, delta, nu, floor_z)); // Create an external constant uniform force shared_ptr uniform_force = make_shared(soft_robots); diff --git a/examples/horton_case/hortonExample.cpp b/examples/horton_case/hortonExample.cpp index d97c298f..065fe318 100755 --- a/examples/horton_case/hortonExample.cpp +++ b/examples/horton_case/hortonExample.cpp @@ -28,13 +28,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); @@ -55,9 +56,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(soft_robots, delta, nu, mu, floor_z)); + forces->addForce(make_shared(soft_robots, delta, nu, floor_z)); // Add kappa_bar controller string phi_ctrl_file_path = "src/controllers/openloop_control_trajectories/horton_controller.csv"; diff --git a/examples/spider_case/spiderExample.cpp b/examples/spider_case/spiderExample.cpp index cfde9544..36dd0779 100755 --- a/examples/spider_case/spiderExample.cpp +++ b/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(soft_robots, delta, nu, mu, floor_z)); + forces->addForce(make_shared(soft_robots, delta, nu, floor_z)); string logfile_base = "log_files/spider"; int logging_period = 20; diff --git a/run_example.sh b/run_example.sh deleted file mode 100644 index b0ee1822..00000000 --- a/run_example.sh +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env bash -cp examples/$1_case/$1Example.cpp robotDescription.cpp -cd build -make -j4 -cd .. - -./dismech.sh $2 \ No newline at end of file diff --git a/run_robot.sh b/run_robot.sh index 9900ebbb..7ea6ddf5 100644 --- a/run_robot.sh +++ b/run_robot.sh @@ -1,7 +1,7 @@ #!/usr/bin/env bash -cp custom_bot/customBot.cpp robotDescription.cpp +cp custom_bot/$1.cpp robotDescription.cpp cd build make -j4 cd .. -./dismech.sh \ No newline at end of file +./dismech.sh $2 \ No newline at end of file diff --git a/src/robotDescription.h b/src/robotDescription.h index 26f11490..6b9ad260 100755 --- a/src/robotDescription.h +++ b/src/robotDescription.h @@ -11,7 +11,6 @@ #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" diff --git a/src/rod_mechanics/external_forces/contactForce.cpp b/src/rod_mechanics/external_forces/contactForce.cpp index 8851e701..c7d6e4cb 100755 --- a/src/rod_mechanics/external_forces/contactForce.cpp +++ b/src/rod_mechanics/external_forces/contactForce.cpp @@ -1,10 +1,11 @@ #include "contactForce.h" #include "time_steppers/baseTimeStepper.h" +//Use Friction boolean flag to set whether friction is used or not contactForce::contactForce(const shared_ptr& m_soft_robots, double m_col_limit, - double m_delta, double m_k_scaler, double m_mu, double m_nu) : + double m_delta, double m_k_scaler, bool friction, double m_nu) : baseForce(m_soft_robots), delta(m_delta), k_scaler(m_k_scaler), - mu(m_mu), nu(m_nu), friction(m_mu > 0.0) + nu(m_nu), friction(friction) { col_detector = make_unique(m_soft_robots, m_col_limit, m_delta); @@ -17,7 +18,7 @@ contactForce::contactForce(const shared_ptr& m_soft_robots, double m e2p_input[9] = K1; e2e_input[12] = K1; - friction_input[36] = mu; + // friction_input[36] = mu; friction_input[38] = K2; sym_eqs = make_unique(); @@ -104,7 +105,8 @@ void contactForce::setupContactVariables(const Vector& 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); @@ -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; } diff --git a/src/rod_mechanics/external_forces/contactForce.h b/src/rod_mechanics/external_forces/contactForce.h index a2373680..8d58aff0 100755 --- a/src/rod_mechanics/external_forces/contactForce.h +++ b/src/rod_mechanics/external_forces/contactForce.h @@ -13,7 +13,7 @@ class contactForce : public baseForce { public: contactForce(const shared_ptr& m_soft_robots, - double m_col_limit, double m_delta, double m_k_scaler, double m_mu, double m_nu); + double m_col_limit, double m_delta, double m_k_scaler, bool friction, double m_nu); // void updateContactStiffness(); void computeForce(double dt) override; diff --git a/src/rod_mechanics/external_forces/floorContactForce.cpp b/src/rod_mechanics/external_forces/floorContactForce.cpp index 331edb92..339ed9a5 100644 --- a/src/rod_mechanics/external_forces/floorContactForce.cpp +++ b/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 floorContactForce::floorContactForce(const shared_ptr& soft_robots, double floor_delta, - double floor_slipTol, double floor_mu, double floor_z) : + double floor_slipTol, double floor_z) : baseForce(soft_robots), delta(floor_delta), slipTol(floor_slipTol), - mu(floor_mu), floor_z(floor_z) + floor_z(floor_z) { K1 = 15 / delta; K2 = 15 / slipTol; @@ -22,7 +22,6 @@ floorContactForce::floorContactForce(const shared_ptr& soft_robots, contact_input[1] = K1; - fric_jacobian_input[5] = mu; fric_jacobian_input[7] = K2; sym_eqs = make_shared(); @@ -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 = limb->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,10 +115,11 @@ 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 = limb->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; @@ -132,7 +127,7 @@ void floorContactForce::computeForceAndJacobian(double dt) { 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; } diff --git a/src/rod_mechanics/external_forces/floorContactForce.h b/src/rod_mechanics/external_forces/floorContactForce.h index a25b2391..030e898e 100644 --- a/src/rod_mechanics/external_forces/floorContactForce.h +++ b/src/rod_mechanics/external_forces/floorContactForce.h @@ -10,13 +10,11 @@ class floorContactForce : public baseForce { public: floorContactForce(const shared_ptr& soft_robots, double floor_delta, - double floor_slipTol, double mu, double floor_z); + double floor_slipTol, double floor_z); ~floorContactForce() override; void computeForce(double dt) override; void computeForceAndJacobian(double dt) override; - // No need - void updateMu(double mu); double min_dist; int num_contacts; @@ -25,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 sym_eqs; Vector contact_input; diff --git a/src/rod_mechanics/external_forces/nonUniformConstantForce.cpp b/src/rod_mechanics/external_forces/nonUniformConstantForce.cpp deleted file mode 100644 index 56edbdc8..00000000 --- a/src/rod_mechanics/external_forces/nonUniformConstantForce.cpp +++ /dev/null @@ -1,56 +0,0 @@ -#include "nonUniformConstantForce.h" -#include "time_steppers/baseTimeStepper.h" -#include - -// Applies a constant force to a specific node of a specific LIMB -// to limbs only, not joints -nonUniformConstantForce::nonUniformConstantForce(const shared_ptr& 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 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); -} \ No newline at end of file diff --git a/src/rod_mechanics/external_forces/nonUniformConstantForce.h b/src/rod_mechanics/external_forces/nonUniformConstantForce.h deleted file mode 100644 index 9e2ca65a..00000000 --- a/src/rod_mechanics/external_forces/nonUniformConstantForce.h +++ /dev/null @@ -1,30 +0,0 @@ -#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& 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 limb_node_forces; -}; - -#endif diff --git a/src/rod_mechanics/softRobots.cpp b/src/rod_mechanics/softRobots.cpp index 36f331cd..3a25a337 100644 --- a/src/rod_mechanics/softRobots.cpp +++ b/src/rod_mechanics/softRobots.cpp @@ -9,14 +9,14 @@ softRobots::~softRobots() = default; void softRobots::addLimb(const Vector3d& start, const Vector3d& end, int num_nodes, double rho, double rod_radius, double youngs_modulus, double poisson_ratio, double mu) { limbs.push_back(make_shared(num_limbs, start, end, num_nodes, rho, - rod_radius, youngs_modulus, poisson_ratio)); + rod_radius, youngs_modulus, poisson_ratio, mu)); num_limbs++; } void softRobots::addLimb(const vector &nodes, double rho, double rod_radius, double youngs_modulus, double poisson_ratio, double mu) { - limbs.push_back(make_shared(num_limbs, nodes, rho, rod_radius, youngs_modulus, poisson_ratio)); + limbs.push_back(make_shared(num_limbs, nodes, rho, rod_radius, youngs_modulus, poisson_ratio, mu)); num_limbs++; } diff --git a/src/rod_mechanics/softRobots.h b/src/rod_mechanics/softRobots.h index b73980ae..847f6c6d 100644 --- a/src/rod_mechanics/softRobots.h +++ b/src/rod_mechanics/softRobots.h @@ -15,9 +15,9 @@ class softRobots ~softRobots(); void addLimb(const Vector3d& start, const Vector3d& end, int num_nodes, - double rho, double rod_radius, double youngs_modulus, double poisson_ratio, double mu); + double rho, double rod_radius, double youngs_modulus, double poisson_ratio, double mu=0.0); void addLimb(const vector& nodes, double rho, double rod_radius, - double youngs_modulus, double poisson_ratio, double mu); + double youngs_modulus, double poisson_ratio, double mu=0.0); void createJoint(int limb_idx, int node_idx); void addToJoint(int joint_idx, int limb_idx, int node_idx);