reloadable motorparams, single testjoint.

src/build/bioloid.cpp

@@ -52,6 +52,7 @@ void Bioloid::create_robot(Robot& robot, int index_number, std::vector<double> p
         case 60: Robots::Nolegs    ::create_worm            (robot); break;
 
         case 80: Robots::Hannah    ::create_hannah_leg      (robot); break;
+        case 81: Robots::HannahRand::create_motor_param_test(robot); break;
 
         case 90: Robots::Standard  ::create_pendulum        (robot); break;
         case 91: Robots::Standard  ::create_double_pendulum (robot); break;

src/build/joints.h

@@ -176,6 +176,8 @@ class NJoint
         pid_maxtorque = common::clip(global_conf.init_max_torque, 0.0, 1.0);
     }
 
+    void reinit_motormodel(ActuatorParameters const& c) { conf = c; }
+
     void draw(void) const;
 
     const unsigned int joint_id;

src/build/params.h

@@ -23,7 +23,7 @@ struct ActuatorParameters {
     , kM               ( motor_parameter::kM      )
     , R_i_inv          ( motor_parameter::R_i_inv )
     {
-        dsPrint("Using standard actuator parameters.");
+        printf("Using standard actuator parameters.");
         static_assert (joint::sticking_friction >= joint::coulomb_friction, "Sticktion must be greater than coulomb friction.");
     }
 
@@ -39,13 +39,64 @@ struct ActuatorParameters {
     , kM               ( rnd(motor_parameter::kM     , perc, var) )
     , R_i_inv          ( rnd(motor_parameter::R_i_inv, perc, var) )
     {
-        dsPrint("Using randomized actuator parameters by %1.2f %% variation", perc * 100 * var);
+        printf("Using randomized actuator parameters by %1.2f %% variation", perc * 100 * var);
         assert (perc >= 0. and perc <= 0.33);
         assert (var  >= 0. and var  <= 1.00);
         /* Sticktion must be greater than coulomb friction. This is guarantied up to a range of 33%*/
         assert (sticking_friction >= coulomb_friction);
     }
 
+    ActuatorParameters(std::vector<double> params, bool assert_range = true)
+    : bristle_displ_max( params.at(0) )
+    , bristle_stiffness( params.at(1) )
+    , sticking_friction( params.at(2) )
+    , coulomb_friction ( params.at(3) )
+    , fluid_friction   ( params.at(4) )
+    , stiction_range   ( params.at(5) )
+    , V_in             ( params.at(6) )
+    , kB               ( params.at(7) )
+    , kM               ( params.at(8) )
+    , R_i_inv          ( params.at(9) )
+    {
+        printf("Using actuator parameters from vector.\n"
+               "bristle_displ_max : %7.4lf | bristle_stiffness : %7.4lf\n"
+               "sticking_friction : %7.4lf | coulomb_friction  : %7.4lf\n"
+               "fluid_friction    : %7.4lf | stiction_range    : %7.4lf\n"
+               "V_in              : %7.4lf | kB                : %7.4lf\n"
+               "kM                : %7.4lf | R_i_inv           : %7.4lf\n"
+              , bristle_displ_max
+              , bristle_stiffness
+              , sticking_friction
+              , coulomb_friction
+              , fluid_friction
+              , stiction_range
+              , V_in
+              , kB
+              , kM
+              , R_i_inv
+              );
+        if (assert_range)
+            assert (sticking_friction >= coulomb_friction);
+    }
+
+    std::vector<double> get(void) const
+    {
+        std::vector<double> params;
+        params.reserve(10);
+        params.emplace_back( bristle_displ_max );
+        params.emplace_back( bristle_stiffness );
+        params.emplace_back( sticking_friction );
+        params.emplace_back( coulomb_friction  );
+        params.emplace_back( fluid_friction    );
+        params.emplace_back( stiction_range    );
+        params.emplace_back( V_in              );
+        params.emplace_back( kB                );
+        params.emplace_back( kM                );
+        params.emplace_back( R_i_inv           );
+        assert(params.size() == 10);
+        return params;
+    }
+
     double bristle_displ_max;
     double bristle_stiffness;
     double sticking_friction;

src/controller/tcp_controller.cpp

@@ -84,6 +84,7 @@ bool TCPController::control(const double time)
 
         /* model updates */
         if (starts_with(msg, "MODEL"  )) { reload_model = parse_update_model_command(msg.c_str()); continue; }
+        if (starts_with(msg, "MOTOR"  )) { parse_update_motor_model(msg.c_str()); continue; }
 
         /* error */
         if (fail_counter++ >= 42) { dsPrint("Too many messages without a 'DONE'-command.\n"); return false; }
@@ -404,43 +405,71 @@ void TCPController::parse_impulse_FI(const char* msg)
     else dsPrint("ERROR: bad 'FI' format: '%s'\n", msg);
 }
 
-bool TCPController::parse_update_model_command(const char* msg)
+std::vector<double> read_params(const char* msg, int* offset, unsigned num_params)
 {
-    int offset = 5;
-    msg += offset;
-
     std::vector<double> params;
-
-    int new_model_id;
-    unsigned num_params;
-
-    if (2 != sscanf(msg, " %d %u%n", &new_model_id, &num_params, &offset)) {
-        dsPrint("ERROR: bad 'MODEL' format: '%s'\n", msg);
-        return false;
-    }
-
-    msg += offset;
     double p = 0.;
 
     if (num_params > 0)
     {
         params.reserve(num_params);
         for (unsigned int idx = 0; idx < num_params; ++idx)
         {
-            if (1 == sscanf(msg, " %lf%n", &p, &offset)) {
-                msg += offset;
+            if (1 == sscanf(msg, " %lf%n", &p, offset)) {
+                msg += (*offset);
                 params.emplace_back(p);
             } else {
                 dsPrint("ERROR: bad parameter format for 'MODEL' : '%s'\n", msg);
-                return false;
+                return {};
             }
         }
     }
+    return params;
+}
+
+bool TCPController::parse_update_model_command(const char* msg)
+{
+    int offset = 5;
+    msg += offset;
+
+    int new_model_id;
+    unsigned num_params;
+
+    if (2 != sscanf(msg, " %d %u%n", &new_model_id, &num_params, &offset)) {
+        dsPrint("ERROR: bad 'MODEL' format: '%s'\n", msg);
+        return false;
+    }
+
+    msg += offset;
+    auto params = read_params(msg, &offset, num_params);
 
     robot.destroy();
     Bioloid::create_robot(robot, new_model_id, params);
     return true;
 }
 
+
+void TCPController::parse_update_motor_model(const char* msg) {
+    int offset = 5;
+    msg += offset;
+
+    unsigned num_params;
+
+    if (1 != sscanf(msg, " %u%n", &num_params, &offset)) {
+        dsPrint("ERROR: bad 'MOTOR' format: '%s'\n", msg);
+        return;
+    }
+
+    msg += offset;
+    auto params = read_params(msg, &offset, num_params);
+
+    dsPrint("Reinitializing actuator model with %u parameters.\n", num_params);
+    for (unsigned int idx = 0; idx < robot.number_of_joints(); ++idx)
+        robot.joints[idx].reinit_motormodel(ActuatorParameters(params));
+
+    return;
+}
+
+
 /* fin */
 

src/controller/tcp_controller.h

@@ -66,6 +66,7 @@ class TCPController : public Controller {
     void parse_impulse_FI(const char* msg);
 
     bool parse_update_model_command(const char* msg);
+    void parse_update_motor_model(const char* msg);
 
     void execute_controller();
 

src/robots/hannah_random.h

@@ -185,6 +185,50 @@ create_random_hannah(Robot& robot, std::vector<double> model_parameter)
 }
 
 
+void
+create_motor_param_test(Robot& robot)
+{
+    const Vector3 size_base (.500, .500, .500);
+    const Vector3 size_rotor(.520, .020, .015);
+
+    dsPrint("HORIZONTAL EXCENTRIC ROTOR FOR MOTOR PARAM TESTLEG\n");
+    Vector3 pos;
+
+    const double joint_distance = 0.005;
+
+    const double axis_delta = 0.0315;
+
+    ActuatorParameters params;
+    params.V_in = 12.0;
+
+    const double density_fagus = 720; /* kg/m^3 */
+
+    /* body */
+    pos.x = .0;
+    pos.y = .0;
+    pos.z = 0.5*size_base.z;
+
+    robot.create_box("base", pos, size_base, .0, constants::materials::heavy, colors::white, true, constants::friction::hi);
+
+    /* arm */
+    pos.x = 0.5*size_rotor.x;
+    pos.y = .0;
+    pos.z = size_base.z + 0.5 * size_rotor.z + joint_distance;
+
+    robot.create_box("rotor", pos, size_rotor, .0, density_fagus, colors::black, false, constants::friction::lo);
+
+    /* connect joints */
+    robot.connect_joint("base" , "rotor", -.5*size_rotor.x + axis_delta, .0, .0, 'Z', -90.0, +90.0, 0.0, JointType::normal, "joint0", "", 5.0, params);
+
+
+    /* attach sensors */
+    //robot.attach_accel_sensor("rotor");
+
+    /* camera */
+    robot.set_camera_center_on("base");
+    robot.setup_camera(Vector3(0.0, -1.20, 1.20), 90, -40, 0);
+}
+
 }} // namespace Robots::Hannah
 
 #endif // HANNAH_RANDOM_H_INCLUDED