adds shared_ptr overloads for mjbots constructors

include/kodlab_mjbots_sdk/common_header.h

@@ -118,4 +118,19 @@ class ValidatedCache
 
 };
 
+/**
+ * @brief Convert vector to shared pointers using copy constructor
+ * @param vect 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

include/kodlab_mjbots_sdk/mjbots_control_loop.h

@@ -13,6 +13,7 @@
 #include "real_time_tools/timer.hpp"
 #include "real_time_tools/hard_spinner.hpp"
 #include "VoidLcm.hpp"
+#include "common_header.h"
 
 namespace kodlab::mjbots {
 /*!
@@ -48,9 +49,14 @@ class MjbotsControlLoop : public AbstractRealtimeObject {
  public:
   /*!
    * @brief constructs and mjbots behavior based on the options struct. Does not Start the controller.
+   * @param joints a std::vector of JointMoteus's (or std::shared_ptr to the same)
    * @param options contains options defining the behavior
    */
   MjbotsControlLoop(std::vector<kodlab::mjbots::JointMoteus> joints, const ControlLoopOptions &options);
+  /*!
+   * \overload 
+   */
+  MjbotsControlLoop(std::vector<std::shared_ptr<kodlab::mjbots::JointMoteus>> joints, const ControlLoopOptions &options);
 
  protected:
 
@@ -111,15 +117,20 @@ class MjbotsControlLoop : public AbstractRealtimeObject {
 /******************************************Implementation**************************************************************/
 
 template<class log_type, class input_type>
-MjbotsControlLoop<log_type, input_type>::MjbotsControlLoop(std::vector<kodlab::mjbots::JointMoteus> joints, const ControlLoopOptions &options) :
+MjbotsControlLoop<log_type, input_type>::MjbotsControlLoop(std::vector<kodlab::mjbots::JointMoteus> joints, const ControlLoopOptions &options)
+  : MjbotsControlLoop( make_share_vector(joints), options){}
+
+template<class log_type, class input_type>
+MjbotsControlLoop<log_type, input_type>::MjbotsControlLoop(std::vector<std::shared_ptr<kodlab::mjbots::JointMoteus>> joint_ptrs, const ControlLoopOptions &options)
+ :
     AbstractRealtimeObject(options.realtime_params.main_rtp, options.realtime_params.can_cpu),
     lcm_sub_(options.realtime_params.lcm_rtp, options.realtime_params.lcm_cpu, options.input_channel_name) {
   // Extract useful values from options
   options_ = options;
   cpu_ = options.realtime_params.main_cpu;
   realtime_priority_ = options.realtime_params.main_rtp;
   frequency_ = options.frequency;
-  num_motors_ = joints.size();
+  num_motors_ = joint_ptrs.size();
   // Setup logging info and confirm template is provided if logging
   logging_channel_name_ = options.log_channel_name;
   logging_ = !logging_channel_name_.empty();
@@ -135,7 +146,7 @@ MjbotsControlLoop<log_type, input_type>::MjbotsControlLoop(std::vector<kodlab::m
   }
 
   // Create robot object
-  robot_ = std::make_shared<MjbotsRobotInterface>(MjbotsRobotInterface(std::move(joints),
+  robot_ = std::make_shared<MjbotsRobotInterface>(MjbotsRobotInterface(joint_ptrs,
                                                                        options_.realtime_params,
                                                                        options_.soft_start_duration,
                                                                        options_.max_torque,

include/kodlab_mjbots_sdk/mjbots_robot_interface.h

@@ -38,7 +38,7 @@ class MjbotsRobotInterface {
 
   /*!
    * @brief constructs an mjbots_robot_interface to communicate with a collection of moeteusses
-   * @param joint_list a list of joints defining the motors in the robot
+   * @param joint_list a list of joints (or shared pointers to joints) defining the motors in the robot
    * @param realtime_params the realtime parameters defining cpu and realtime priority
    * @param soft_start_duration how long in dt to spend ramping the torque
    * @param robot_max_torque the maximum torque to allow per motor in the robot
@@ -53,6 +53,16 @@ class MjbotsRobotInterface {
                        ::mjbots::pi3hat::Euler imu_mounting_deg = ::mjbots::pi3hat::Euler(),
                        int imu_rate_hz = 1000,
                        ::mjbots::pi3hat::Euler imu_world_offset_deg = ::mjbots::pi3hat::Euler());
+  /*!
+   * \overload
+   */
+  MjbotsRobotInterface(std::vector<std::shared_ptr<JointMoteus>> joint_list,
+                       const RealtimeParams &realtime_params,
+                       int soft_start_duration = 1,
+                       float robot_max_torque = 100,
+                       ::mjbots::pi3hat::Euler imu_mounting_deg = ::mjbots::pi3hat::Euler(),
+                       int imu_rate_hz = 1000,
+                       ::mjbots::pi3hat::Euler imu_world_offset_deg = ::mjbots::pi3hat::Euler());
 
   /**
    * @brief Send and recieve initial communications effectively starting the robot

src/mjbots_robot_interface.cpp

@@ -54,16 +54,21 @@ MjbotsRobotInterface::MjbotsRobotInterface(const std::vector<JointMoteus> &joint
                                            ::mjbots::pi3hat::Euler imu_mounting_deg,
                                            int imu_rate_hz,
                                            ::mjbots::pi3hat::Euler imu_world_offset_deg)
+    : MjbotsRobotInterface(make_share_vector(joint_list), realtime_params, soft_start_duration, robot_max_torque) {}
+
+MjbotsRobotInterface::MjbotsRobotInterface(std::vector<std::shared_ptr<JointMoteus>> joint_ptrs,
+                                           const RealtimeParams &realtime_params,
+                                           int soft_start_duration,
+                                           float robot_max_torque,
+                                           ::mjbots::pi3hat::Euler imu_mounting_deg,
+                                           int imu_rate_hz,
+                                           ::mjbots::pi3hat::Euler imu_world_offset_deg)
     : soft_start_(robot_max_torque, soft_start_duration)
 {
+    joints = joint_ptrs;
 
-  for (JointMoteus joint: joint_list){
-    // Make vector of shared_pointer objects using joint copy constructer 
-    // (original list will be descoped)
-    joints.push_back(std::make_shared<JointMoteus>(joint)); 
-  }
-
-  for( auto & j: joints){
+  for (auto &j : joints)
+  {
     positions_.push_back( j->get_position_reference() );
     velocities_.push_back( j->get_velocity_reference() );
     torque_cmd_.push_back( j->get_servo_torque_reference()   ); 
@@ -96,10 +101,8 @@ MjbotsRobotInterface::MjbotsRobotInterface(const std::vector<JointMoteus> &joint
   moteus_data_.commands = {commands_.data(), commands_.size()};
   moteus_data_.replies = {replies_.data(), replies_.size()};
   moteus_data_.timeout = timeout_;
-
 }
 
-
 void MjbotsRobotInterface::Init() {
     SendCommand();
     ProcessReply();