diff --git a/.gitignore b/.gitignore index 4cebd082..f33819bd 100644 --- a/.gitignore +++ b/.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 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/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 ecb3bca8..07cd6d80 100755 --- a/examples/horton_case/hortonExample.cpp +++ b/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(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_robot.sh b/run_robot.sh new file mode 100644 index 00000000..7ea6ddf5 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/rod_mechanics/elasticRod.cpp b/src/rod_mechanics/elasticRod.cpp index af42c144..ecfcc538 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 f0360e52..f392e624 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); @@ -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; diff --git a/src/rod_mechanics/external_forces/contactForce.cpp b/src/rod_mechanics/external_forces/contactForce.cpp index 2886c65a..3a756702 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" + contactForce::contactForce(const shared_ptr& 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(soft_robots, col_limit, delta, self_contact); @@ -17,7 +18,7 @@ contactForce::contactForce(const shared_ptr& 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(); @@ -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); @@ -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; } diff --git a/src/rod_mechanics/external_forces/contactForce.h b/src/rod_mechanics/external_forces/contactForce.h index 4ba9a6d4..cb9537f1 100755 --- a/src/rod_mechanics/external_forces/contactForce.h +++ b/src/rod_mechanics/external_forces/contactForce.h @@ -13,7 +13,8 @@ class contactForce : public baseForce { public: contactForce(const shared_ptr& 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; diff --git a/src/rod_mechanics/external_forces/floorContactForce.cpp b/src/rod_mechanics/external_forces/floorContactForce.cpp index 331edb92..ba3b4be8 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 if floor mu is not specified 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, 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& 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 = (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,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 = (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; @@ -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 5c698663..b6305e3b 100644 --- a/src/rod_mechanics/external_forces/floorContactForce.h +++ b/src/rod_mechanics/external_forces/floorContactForce.h @@ -10,12 +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, 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 sym_eqs; Vector contact_input; @@ -35,7 +34,7 @@ class floorContactForce : public baseForce Vector ffr; int fric_jaco_type; double contact_stiffness; - double mu; + double floor_mu; double delta; double slipTol; double orig_slip_tol; diff --git a/src/rod_mechanics/softRobots.cpp b/src/rod_mechanics/softRobots.cpp index f5d37a98..5074b8c2 100644 --- a/src/rod_mechanics/softRobots.cpp +++ b/src/rod_mechanics/softRobots.cpp @@ -7,16 +7,16 @@ 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)); + 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) { - limbs.push_back(make_shared(num_limbs, nodes, rho, rod_radius, youngs_modulus, poisson_ratio)); + double poisson_ratio, double mu) { + limbs.push_back(make_shared(num_limbs, nodes, rho, rod_radius, youngs_modulus, poisson_ratio, mu)); num_limbs++; } @@ -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()) { diff --git a/src/rod_mechanics/softRobots.h b/src/rod_mechanics/softRobots.h index 29518151..b64b3571 100644 --- a/src/rod_mechanics/softRobots.h +++ b/src/rod_mechanics/softRobots.h @@ -17,9 +17,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=0.0); void addLimb(const vector& nodes, double rho, double rod_radius, - double youngs_modulus, double poisson_ratio); + 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);