Separate RobotBase from MJBotsInterface

examples/CMakeLists.txt

@@ -10,5 +10,6 @@ endmacro(add_example demo_name)
 add_example(leg_2DoF_example)
 add_example(leg_3DoF_example)
 add_example(spin_joints_example)
+add_example(imu_example)
 add_example(proprio_example)
-add_example(imu_example)
+add_example(robot_example)

examples/imu_example.cpp

@@ -27,7 +27,7 @@ class AttitudeExample : public kodlab::mjbots::MjbotsControlLoop<IMULog,
    * @note This is is a shared pointer to a continually updated `IMUData`
    *       object.  There is no need to update in the control loop itself.
    */
-  const std::shared_ptr<kodlab::IMUData<float>> imu_ = robot_->GetIMUDataSharedPtr();
+  const std::shared_ptr<kodlab::IMUData<float>> imu_ = mjbots_interface_->GetIMUDataSharedPtr();
 
   /**
    * @brief Read-only IMU data object
@@ -43,7 +43,7 @@ class AttitudeExample : public kodlab::mjbots::MjbotsControlLoop<IMULog,
   void CalcTorques() override
   {
     // Update Read-Only IMUData
-    imu_read_only_ = robot_->GetIMUData();
+    imu_read_only_ = mjbots_interface_->GetIMUData();
 
     // Read Various IMUData Information from `imu_`
     const Eigen::Quaternionf quat = imu_->get_quat();    // attitude quaternion

examples/leg_2DoF_example.cpp

@@ -88,7 +88,7 @@ class Hopping : public kodlab::mjbots::MjbotsControlLoop<LegLog, LegGains> {
     for (int servo = 0; servo < 2; servo++) {
       log_data_.positions[servo] = robot_->GetJointPositions()[servo];
       log_data_.velocities[servo] = robot_->GetJointVelocities()[servo];
-      log_data_.modes[servo] = static_cast<int>(robot_->GetJointModes()[servo]);
+      log_data_.modes[servo] = static_cast<int>(mjbots_interface_->GetJointModes()[servo]);
       log_data_.torque_cmd[servo] = robot_->GetJointTorqueCmd()[servo];
     }
     log_data_.limb_position[0] = z_ - z0_;

examples/leg_3DoF_example.cpp

@@ -45,7 +45,7 @@ class Joints3DoF : public kodlab::mjbots::MjbotsControlLoop<ManyMotorLog> {
     for (int servo = 0; servo < num_motors_; servo++) {
       log_data_.positions[servo]  = robot_->GetJointPositions()[servo];
       log_data_.velocities[servo] = robot_->GetJointVelocities()[servo];
-      log_data_.modes[servo] = static_cast<int>(robot_->GetJointModes()[servo]);
+      log_data_.modes[servo] = static_cast<int>(mjbots_interface_->GetJointModes()[servo]);
       log_data_.torques[servo] = robot_->GetJointTorqueCmd()[servo];
     }
     for (int servo = num_motors_; servo < 13; servo++) {

examples/proprio_example.cpp

@@ -63,7 +63,7 @@ class ProprioJoints : public kodlab::mjbots::MjbotsControlLoop<ManyMotorLog> {
     for (int servo = 0; servo < num_motors_; servo++) {
       log_data_.positions[servo]  = robot_->GetJointPositions()[servo];
       log_data_.velocities[servo] = robot_->GetJointVelocities()[servo];
-      log_data_.modes[servo] = static_cast<int>(robot_->GetJointModes()[servo]);
+      log_data_.modes[servo] = static_cast<int>(mjbots_interface_->GetJointModes()[servo]);
       log_data_.torques[servo] = robot_->GetJointTorqueCmd()[servo];
     }
     for (int servo = num_motors_; servo < 13; servo++) {

examples/robot_example.cpp

@@ -0,0 +1,82 @@
+/*!
+ * @file robot_example.cpp
+ * @author J. Diego Caporale <[email protected]>
+ * @brief Example that uses SimpleRobot(a RobotInterfaceDerived class defined in a seperate header file) separating the 
+ *        controller, state update, and state machine from the lower lever control loop. 
+ * @date 2022-07-17
+ * 
+ * @copyright Copyright (c) 2021 The Trustees of the University of Pennsylvania. All Rights Reserved
+ *            BSD 3-Clause License
+ * 
+ */
+#include <vector>
+#include <iostream>
+#include "kodlab_mjbots_sdk/common_header.h"
+#include "ManyMotorLog.hpp"
+#include "ModeInput.hpp"
+#include "kodlab_mjbots_sdk/robot_interface.h"
+#include "kodlab_mjbots_sdk/mjbots_control_loop.h"
+
+#include "examples/simple_robot.h"
+
+class SimpleRobotControlLoop : public kodlab::mjbots::MjbotsControlLoop<ManyMotorLog, ModeInput, SimpleRobot>
+{
+    using MjbotsControlLoop::MjbotsControlLoop;
+
+    void CalcTorques() override
+    {
+        robot_->Update();
+    }
+    void PrepareLog() override
+    {
+        for (int servo = 0; servo < num_motors_; servo++)
+        {
+            log_data_.positions[servo] = robot_->GetJointPositions()[servo];
+            log_data_.velocities[servo] = robot_->GetJointVelocities()[servo];
+            log_data_.modes[servo] = static_cast<int>(mjbots_interface_->GetJointModes()[servo]);
+            log_data_.torques[servo] = robot_->GetJointTorqueCmd()[servo];
+        }
+        for (int servo = num_motors_; servo < 13; servo++)
+        {
+            log_data_.positions[servo] = 0;
+            log_data_.velocities[servo] = 0;
+            log_data_.modes[servo] = 0;
+            log_data_.torques[servo] = 0;
+        }
+    }
+
+    void ProcessInput() override
+    {
+        robot_->mode = lcm_sub_.data_.mode;
+        std::cout << "Switching to behavior " << robot_->mode << std::endl;
+        // If the kill robot mode is detected kill robot using CTRL_C flag handler.
+        if (robot_->mode == robot_->KILL_ROBOT)
+        { // KILL_ROBOT
+            kodlab::CTRL_C_DETECTED = true;
+        }
+    }
+};
+
+int main(int argc, char **argv)
+{
+    // Setup joints with a std::vector or JointSharedVector class
+    std::vector<kodlab::mjbots::JointMoteus> joints;
+    joints.emplace_back(100, 4, 1, 0,   1, 0);
+    joints.emplace_back(101, 4,-1, 0, 5.0/3.0, 0);
+    // JointSharedVector<kodlab::mjbots::JointMoteus> joints;
+    // joints.addJoint(100, 4, 1, 0, 1, 0);
+    // joints.addJoint(101, 4, -1, 0, 5.0 / 3.0, 0);
+
+    // Define robot options
+    kodlab::mjbots::ControlLoopOptions options;
+    options.log_channel_name = "motor_data";
+    options.frequency = 1000;
+    options.realtime_params.main_cpu = 3;
+    options.realtime_params.can_cpu = 2;
+
+    // Create control loop
+    // Starts the loop, and then join it
+    SimpleRobotControlLoop simple_robot(joints, options);
+    simple_robot.Start();
+    simple_robot.Join();
+}

examples/spin_joints_example.cpp

@@ -27,7 +27,7 @@ class Spin_Joint : public kodlab::mjbots::MjbotsControlLoop<ManyMotorLog> {
     for (int servo = 0; servo < num_motors_; servo++) {
       log_data_.positions[servo] = robot_->GetJointPositions()[servo];
       log_data_.velocities[servo] = robot_->GetJointVelocities()[servo];
-      log_data_.modes[servo] = static_cast<int>(robot_->GetJointModes()[servo]);
+      log_data_.modes[servo] = static_cast<int>(mjbots_interface_->GetJointModes()[servo]);
       log_data_.torques[servo] = robot_->GetJointTorqueCmd()[servo];
     }
     for (int servo = num_motors_; servo < 13; servo++) {

include/examples/simple_robot.h

@@ -0,0 +1,29 @@
+/**
+ * @file simple_robot.h
+ * @author J. Diego Caporale
+ * @brief A simple RobotInterface derived class example. Here is where you would implement any state updates,
+ *        behaviors, or control system
+ * @date 2022-07-19
+ *
+ * @copyright Copyright (c) 2022
+ *
+ */
+
+#pragma once
+
+#include "kodlab_mjbots_sdk/robot_interface.h"
+
+class SimpleRobot : virtual public kodlab::RobotInterface
+{
+
+    using kodlab::RobotInterface::RobotInterface;
+
+public:
+    int mode = 0; // Member variables encode whatever added state we need
+    // Set up robot update function for state and torques
+    void Update() override
+    {
+        std::vector<float> torques(num_joints_, 0);
+        SetTorques(torques);
+    }
+};

include/kodlab_mjbots_sdk/common_header.h

@@ -5,6 +5,8 @@
 #include <signal.h>  // manage the ctrl+c signal
 #include <type_traits>
 #include <iostream>
+#include <memory>
+#include <vector>
 
 namespace kodlab {
 /*
@@ -118,4 +120,19 @@ class ValidatedCache
 
 };
 
+/**
+ * @brief Convert vector to shared pointers using copy constructor
+ * @param objs vector of type T
+ * @tparam T object type
+ */
+
+template< typename T>
+std::vector<std::shared_ptr <T> > make_share_vector(std::vector<T> objs){
+  std::vector<std::shared_ptr<T>> ptrs;
+  for (T obj: objs){
+    // Make vector of shared_pointer objects using copy constructer 
+    ptrs.push_back(std::make_shared<T>(obj)); 
+  }
+  return ptrs;
+}
 }  // namespace kodlab