correct mass properties for the sim

include/MoeModel.hpp

@@ -32,23 +32,23 @@ class MoeModel {
     double q0dd, q1dd, q2dd, q3dd;
 
     // Hardstops
-    const double q0min = -75.0 * mahi::util::DEG2RAD;
-    const double q0max = +30.0 * mahi::util::DEG2RAD;
-    const double q1min = -90.0 * mahi::util::DEG2RAD;
-    const double q1max = +90.0 * mahi::util::DEG2RAD;
-    const double q2min = -90.0 * mahi::util::DEG2RAD;
-    const double q2max = +90.0 * mahi::util::DEG2RAD;
-    const double q3min = -60.0 * mahi::util::DEG2RAD;
-    const double q3max = +15.0 * mahi::util::DEG2RAD;
-
-    double Khard0 = 20000; // hardstop stiffness
-    double Bhard0 = 100;  // hardstop damping
-    double Khard1 = 2000; // hardstop stiffness
-    double Bhard1 = 100;  // hardstop damping
-    double Khard2 = 2000; // hardstop stiffness
-    double Bhard2 = 100;  // hardstop damping
-    double Khard3 = 2000; // hardstop stiffness
-    double Bhard3 = 100;  // hardstop damping
+    const double q0min = -73.6285 * mahi::util::DEG2RAD;
+    const double q0max = +28.6283 * mahi::util::DEG2RAD;
+    const double q1min = -89.8249 * mahi::util::DEG2RAD;
+    const double q1max = +89.8249 * mahi::util::DEG2RAD;
+    const double q2min = -63.2490 * mahi::util::DEG2RAD;
+    const double q2max = +68.2490 * mahi::util::DEG2RAD;
+    const double q3min = -72.9322 * mahi::util::DEG2RAD;
+    const double q3max = +05.0000 * mahi::util::DEG2RAD;
+
+    double Khard0 = 100; // hardstop stiffness
+    double Bhard0 = 1.0;  // hardstop damping
+    double Khard1 = 100; // hardstop stiffness
+    double Bhard1 = 1.0;  // hardstop damping
+    double Khard2 = 100; // hardstop stiffness
+    double Bhard2 = 1.0;  // hardstop damping
+    double Khard3 = 100; // hardstop stiffness
+    double Bhard3 = 1.0;  // hardstop damping
 
     double mat_calc_time = 0;
     double setup_time = 0;
@@ -71,16 +71,16 @@ class MoeModel {
     const double eta3 = 0.23*0.0254;
 
     // Damping Coefficients [Nm*s/rad]
-    const double b0 = 0.5 * 0.0252;  
-    const double b1 = 0.5 * 0.0019;  
-    const double b2 = 0.5 * 0.0029; 
-    const double b3 = 0.5 * 0.0019;  
+    // const double b0 = 0.5 * 0.0252;  
+    // const double b1 = 0.5 * 0.0019;  
+    // const double b2 = 0.5 * 0.0029; 
+    // const double b3 = 0.5 * 0.0019;  
 
     // Kinetic Friction [Nm]
-    const double fk0 = 0.5 * 0.1891;
-    const double fk1 = 0.5 * 0.0541;
-    const double fk2 = 0.5 * 0.1339;
-    const double fk3 = 0.5 * 0.1339;
+    // const double fk0 = 0.5 * 0.1891;
+    // const double fk1 = 0.5 * 0.0541;
+    // const double fk2 = 0.5 * 0.1339;
+    // const double fk3 = 0.5 * 0.1339;
 
     // Gravity Constant [m/s^2]
     const double g = 9.80665;

include/mass_properties.hpp

@@ -3,47 +3,47 @@
 
 constexpr double g = 9.80665;
 
-constexpr double Pcx0 = 0.240075618406;
-constexpr double Pcy0 = 0.313243565004;
-constexpr double Pcz0 = -0.0020437124536;
-constexpr double m0 = 1.66430088983;
-constexpr double Icxx0 = 0.173060333455;
-constexpr double Icyy0 = 0.105708558223;
-constexpr double Iczz0 = 0.267640146896;
-constexpr double Icxy0 = 0.12253097199;
-constexpr double Icxz0 = -0.00075342842528;
-constexpr double Icyz0 = -0.000953579998468;
+constexpr double Pcx0 = -0.0700937604457;
+constexpr double Pcy0 = -0.0296295482409;
+constexpr double Pcz0 = -0.00310094611151;
+constexpr double m0 = 1.98891333721;
+constexpr double Icxx0 = 0.0125262604464;
+constexpr double Icyy0 = 0.0207070557546;
+constexpr double Iczz0 = 0.0206112341895;
+constexpr double Icxy0 = 0.00689201736844;
+constexpr double Icxz0 = 0.000540253128811;
+constexpr double Icyz0 = 7.14491990906e-05;
 
-constexpr double Pcx1 = 0.339954202214;
-constexpr double Pcy1 = 0.354474877058;
-constexpr double Pcz1 = -0.025448595209;
-constexpr double m1 = 1.56688172243;
-constexpr double Icxx1 = 0.207228903876;
-constexpr double Icyy1 = 0.189827318755;
-constexpr double Iczz1 = 0.383322766765;
-constexpr double Icxy1 = 0.18856549491;
-constexpr double Icxz1 = -0.0141241531936;
-constexpr double Icyz1 = -0.0153207901688;
+constexpr double Pcx1 = -4.80999005199e-06;
+constexpr double Pcy1 = 0.0499178775183;
+constexpr double Pcz1 = 0.121392926268;
+constexpr double m1 = 0.994272710768;
+constexpr double Icxx1 = 0.0210296363659;
+constexpr double Icyy1 = 0.0202934151143;
+constexpr double Iczz1 = 0.00877988910227;
+constexpr double Icxy1 = -3.98254768634e-07;
+constexpr double Icxz1 = -1.44368827065e-07;
+constexpr double Icyz1 = 0.00536792689384;
 
-constexpr double Pcx2 = 0.429473728315;
-constexpr double Pcy2 = 0.308145407542;
-constexpr double Pcz2 = 0.0130756679615;
-constexpr double m2 = 0.816559966496;
-constexpr double Icxx2 = 0.0823772699252;
-constexpr double Icyy2 = 0.156896056801;
-constexpr double Iczz2 = 0.230207053587;
-constexpr double Icxy2 = 0.108171858138;
-constexpr double Icxz2 = 0.00517073289558;
-constexpr double Icyz2 = 0.00356702867429;
+constexpr double Pcx2 = 0.0130715727106;
+constexpr double Pcy2 = -0.0235629254455;
+constexpr double Pcz2 = -0.0792204242197;
+constexpr double m2 = 0.816517756852;
+constexpr double Icxx2 = 0.0076368068358;
+constexpr double Icyy2 = 0.00996590516133;
+constexpr double Iczz2 = 0.00673638689845;
+constexpr double Icxy2 = 0.00033364136615;
+constexpr double Icxz2 = -0.000568696152282;
+constexpr double Icyz2 = 0.00163215324534;
 
-constexpr double Pcx3 = 0.510850608852;
-constexpr double Pcy3 = 0.395976337982;
-constexpr double Pcz3 = 0.0542505401696;
-constexpr double m3 = 0.189775806337;
-constexpr double Icxx3 = 0.0307610169203;
-constexpr double Icyy3 = 0.0505318282735;
-constexpr double Iczz3 = 0.0795396234069;
-constexpr double Icxy3 = 0.038388427226;
-constexpr double Icxz3 = 0.00518646222678;
-constexpr double Icyz3 = 0.00396596150177;
+constexpr double Pcx3 = 0.0528550087023;
+constexpr double Pcy3 = 0.00051002595098;
+constexpr double Pcz3 = 0.0555002362428;
+constexpr double m3 = 0.205942139945;
+constexpr double Icxx3 = 0.00122907259713;
+constexpr double Icyy3 = 0.00171842440604;
+constexpr double Iczz3 = 0.00102904728027;
+constexpr double Icxy3 = 9.44731223253e-05;
+constexpr double Icxz3 = 0.000513711694943;
+constexpr double Icyz3 = -0.000134523250356;
 

src/MoeModel.cpp

@@ -57,13 +57,13 @@ void MoeModel::update(Time t)
     Tau[2] = tau2 + hardstop_torque(q2,q2d,q2min,q2max,Khard2,Bhard2);
     Tau[3] = tau3 + hardstop_torque(q3,q3d,q3min,q3max,Khard3,Bhard3);
 
-    constexpr double B_coef[4] = {0.5, 0.1, 0.1, 0.1};
-    constexpr double Fk_coef[4] = {0.1, 0.01, 0.01, 0.01};
+    constexpr double  B_coef[4] = {0.0252, 0.0019, 0.0029, 0.0019};
+    constexpr double Fk_coef[4] = {   0.2, 0.1891, 0.0541, 0.1339};
 
-    B[0] = B_coef[0]*q0d*10.0;
-    B[1] = B_coef[1]*q1d*10.0;
-    B[2] = B_coef[2]*q2d*10.0;
-    B[3] = B_coef[3]*q3d*10.0;
+    B[0] = B_coef[0]*q0d*1.0;
+    B[1] = B_coef[1]*q1d*1.0;
+    B[2] = B_coef[2]*q2d*1.0;
+    B[3] = B_coef[3]*q3d*1.0;
 
     Fk[0] = Fk_coef[0]*std::tanh(q0d*10);
     Fk[1] = Fk_coef[1]*std::tanh(q1d*10);

src/dll.cpp

@@ -30,8 +30,8 @@ void simulation()
     double q_ref3 = 0.0;
     double q0, q1, q2, q3;
     double tau0, tau1, tau2, tau3;
-    double k_hard0 = 200;
-    double b_hard0 = 10;
+    double k_hard0 = 100;
+    double b_hard0 = 1.0;
     double threadpooling = true;
     Timer timer(hertz(1000), Timer::Hybrid);
     Time t;

src/tuner.cpp

@@ -67,23 +67,23 @@ class SimTuner : public Application {
         ImGui::Begin("PD Tuner");
         ImGui::BeginGroup();
             ImGui::PushItemWidth(ImGui::GetWindowWidth() * 0.3f);
-            ImGui::DragDouble("Kp Elbow",&kp0,0,0,50);
-            ImGui::DragDouble("Kd Elbow",&kd0,0,0,10);
-            ImGui::DragDouble("Kp Forearm",&kp1,0,0,50);
-            ImGui::DragDouble("Kd Forearm",&kd1,0,0,10);
-            ImGui::DragDouble("Kp Wrist FE",&kp2,0,0,1000);
-            ImGui::DragDouble("Kd Wrist FE",&kd2,0,0,100);
-            ImGui::DragDouble("Kp Wrist RU",&kp3,0,0,1000);
-            ImGui::DragDouble("Kd Wrist RU",&kd3,0,0,100);
-            ImGui::DragDouble("Khard",&k_hard,0,0,20000);
-            ImGui::DragDouble("Bhard",&b_hard,0,0,2000);
+            ImGui::DragDouble("Kp Elbow",&kp0,1,0,250);
+            ImGui::DragDouble("Kd Elbow",&kd0,1,0,10);
+            ImGui::DragDouble("Kp Forearm",&kp1,1,0,50);
+            ImGui::DragDouble("Kd Forearm",&kd1,1,0,10);
+            ImGui::DragDouble("Kp Wrist FE",&kp2,1,0,50);
+            ImGui::DragDouble("Kd Wrist FE",&kd2,1,0,10);
+            ImGui::DragDouble("Kp Wrist RU",&kp3,1,0,50);
+            ImGui::DragDouble("Kd Wrist RU",&kd3,1,0,10);
+            ImGui::DragDouble("Khard",&k_hard,1,0,20000);
+            ImGui::DragDouble("Bhard",&b_hard,1,0,2000);
         ImGui::EndGroup();
         ImGui::SameLine();
         ImGui::BeginGroup();
-            ImGui::DragDouble("Elbow ref (rad)",&q_ref0,0.005f,-PI/2-0.5,0.5,"%.4f");
-            ImGui::DragDouble("Forearm ref (rad)",&q_ref1,0.005f,-PI,PI,"%.4f");
-            ImGui::DragDouble("Wrist FE ref (rad)",&q_ref2,0.005f,-PI/2,PI/2,"%.4f");    
-            ImGui::DragDouble("Wrist RU ref (rad)",&q_ref3,0.005f,-PI/2,PI/2,"%.4f");   
+            ImGui::DragDouble("Elbow ref (rad)",&q_ref0,0.005f,-PI/2-0.5,PI/6+0.5,"%.4f");
+            ImGui::DragDouble("Forearm ref (rad)",&q_ref1,0.005f,-PI-0.5,PI+0.5,"%.4f");
+            ImGui::DragDouble("Wrist FE ref (rad)",&q_ref2,0.005f,-PI/2-0.5,PI/2+0.5,"%.4f");    
+            ImGui::DragDouble("Wrist RU ref (rad)",&q_ref3,0.005f,-PI/2-0.5,PI/6+0.5,"%.4f");   
             ImGui::DragDouble("Elbow pos (rad)",&q0,0.0001f,0.03,0.15,"%.4f");
             ImGui::DragDouble("Forearm pos (rad)",&q1,0.0001f,0.03,0.15,"%.4f");    
             ImGui::DragDouble("Wrist FE pos (m)",&q2,0.0001f,0.03,0.15,"%.4f");
@@ -128,8 +128,8 @@ class SimTuner : public Application {
     float t = 0;
     bool tp = true;
     bool enabled = false;
-    double k_hard = 200;
-    double b_hard = 10;
+    double k_hard = 100;
+    double b_hard = 1.0;
     double kp0 = 125.0;
     double kd0 = 1.75;
     double kp1 = 25;

unity/MOE_sim/Assets/Scripts/MOE_Script.cs

@@ -23,8 +23,8 @@ public class MOE_Script : MonoBehaviour {
 
     double[] qs = new double[4];
 
-    const string import_module = "tuner_moe";
-    // const string import_module = "virtual_moe";
+    //const string import_module = "tuner_moe";
+     const string import_module = "virtual_moe";
 
     static IntPtr nativeLibraryPtr;