diff --git a/include/kodlab_mjbots_sdk/common_header.h b/include/kodlab_mjbots_sdk/common_header.h index 6bd3823d..11d7db90 100644 --- a/include/kodlab_mjbots_sdk/common_header.h +++ b/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 > make_share_vector(std::vector objs){ + std::vector> ptrs; + for (T obj: objs){ + // Make vector of shared_pointer objects using copy constructer + ptrs.push_back(std::make_shared(obj)); + } + return ptrs; +} } // namespace kodlab \ No newline at end of file diff --git a/include/kodlab_mjbots_sdk/mjbots_control_loop.h b/include/kodlab_mjbots_sdk/mjbots_control_loop.h index 1325ec21..20e5eeab 100644 --- a/include/kodlab_mjbots_sdk/mjbots_control_loop.h +++ b/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 joints, const ControlLoopOptions &options); + /*! + * \overload + */ + MjbotsControlLoop(std::vector> joints, const ControlLoopOptions &options); protected: @@ -111,7 +117,12 @@ class MjbotsControlLoop : public AbstractRealtimeObject { /******************************************Implementation**************************************************************/ template -MjbotsControlLoop::MjbotsControlLoop(std::vector joints, const ControlLoopOptions &options) : +MjbotsControlLoop::MjbotsControlLoop(std::vector joints, const ControlLoopOptions &options) + : MjbotsControlLoop( make_share_vector(joints), options){} + +template +MjbotsControlLoop::MjbotsControlLoop(std::vector> 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 @@ -119,7 +130,7 @@ MjbotsControlLoop::MjbotsControlLoop(std::vector::MjbotsControlLoop(std::vector(MjbotsRobotInterface(std::move(joints), + robot_ = std::make_shared(MjbotsRobotInterface(joint_ptrs, options_.realtime_params, options_.soft_start_duration, options_.max_torque, diff --git a/include/kodlab_mjbots_sdk/mjbots_robot_interface.h b/include/kodlab_mjbots_sdk/mjbots_robot_interface.h index d59f44dd..9a107d98 100644 --- a/include/kodlab_mjbots_sdk/mjbots_robot_interface.h +++ b/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> 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 diff --git a/src/mjbots_robot_interface.cpp b/src/mjbots_robot_interface.cpp index c96e6b6a..bda5bf5a 100644 --- a/src/mjbots_robot_interface.cpp +++ b/src/mjbots_robot_interface.cpp @@ -54,16 +54,21 @@ MjbotsRobotInterface::MjbotsRobotInterface(const std::vector &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> 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(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 &joint moteus_data_.commands = {commands_.data(), commands_.size()}; moteus_data_.replies = {replies_.data(), replies_.size()}; moteus_data_.timeout = timeout_; - } - void MjbotsRobotInterface::Init() { SendCommand(); ProcessReply();