From ef4273e784d6cbb37c4e6f1bfdfcf58079e9d3ba Mon Sep 17 00:00:00 2001 From: ThomasDuvinage Date: Mon, 17 Feb 2025 22:13:13 +0900 Subject: [PATCH] make mc_rtde realtime : initial commit --- src/CMakeLists.txt | 4 +- src/MCControlRtde.cpp | 178 --------------------- src/MCControlRtde.h | 52 ------ src/URControl.cpp | 356 ++++++++++++++++++++++++++---------------- src/URControl.h | 248 +++++++++++++---------------- src/URControlType.h | 114 ++++++++++++++ src/main.cpp | 118 +++++++------- src/thread.h | 14 ++ 8 files changed, 514 insertions(+), 570 deletions(-) delete mode 100644 src/MCControlRtde.cpp delete mode 100644 src/MCControlRtde.h create mode 100644 src/URControlType.h create mode 100644 src/thread.h diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 484779f..227819b 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -5,9 +5,9 @@ find_package(mc_rtc REQUIRED) find_package(Boost REQUIRED COMPONENTS program_options thread) find_package(ur_rtde REQUIRED) -set(HEADERS MCControlRtde.h URControl.h ControlMode.h) +set(HEADERS URControlType.h URControl.h ControlMode.h thread.h) -set(SOURCES main.cpp URControl.cpp MCControlRtde.cpp) +set(SOURCES main.cpp URControl.cpp) add_executable(${PROGRAM_NAME} ${SOURCES} ${HEADERS}) target_link_libraries(${PROGRAM_NAME} PUBLIC mc_rtc::mc_control diff --git a/src/MCControlRtde.cpp b/src/MCControlRtde.cpp deleted file mode 100644 index 89254b9..0000000 --- a/src/MCControlRtde.cpp +++ /dev/null @@ -1,178 +0,0 @@ -// SpaceVecAlg -#include - -#include - -// mc_rtde -#include "MCControlRtde.h" - -namespace mc_rtde -{ - -MCControlRtde::MCControlRtde(mc_control::MCGlobalController & controller, const std::string & host) -: globalController_(controller), host_(host), robot_(nullptr), - logger_(mc_rtc::Logger::Policy::THREADED, "/tmp", "mc_rtde-" + controller.robot().name()), delay_(0.0) -{ - // Get parameter values from the configuration file - URConfigParameter config_param; - auto rtdeConfig = controller.configuration().config("RTDE"); - if(!rtdeConfig.has("JointSpeed")) - { - mc_rtc::log::warning("[mc_rtde] 'JointSpeed' config entry missing. Using default value: {}", - config_param.joint_speed_); - } - rtdeConfig("JointSpeed", config_param.joint_speed_); - - if(!rtdeConfig.has("JointAcceleration")) - { - mc_rtc::log::warning("[mc_rtde] 'JointAcceleration' config entry missing. Using default value: {}", - config_param.joint_acceleration_); - } - rtdeConfig("JointAcceleration", config_param.joint_acceleration_); - - auto robot_name = controller.robot().name(); - if(!rtdeConfig.has(robot_name)) - { - mc_rtc::log::error( - "[mc_rtde] A name that matches the controller robot name is not defined in the configuration file"); - return; - } - - /* Get communication information */ - mc_rtc::Configuration config_robot = rtdeConfig(robot_name); - if(!config_robot.has("IP")) - { - mc_rtc::log::warning("[mc_rtde] 'IP' config entry missing. Using default value: {}", config_param.targetIP_); - } - config_robot("IP", config_param.targetIP_); - - /* Connect to robot (real or simulation) */ - if(host != "simulation") - { - /* Try to connect via UDP to the robot */ - mc_rtc::log::info("[mc_rtde] Connecting to {} robot on address {}", robot_name, config_param.targetIP_); - - robot_ = new URControl(config_param); - - /* Get the sensor value once */ - if(!robot_->getState(state_)) - { - mc_rtc::log::error_and_throw("[mc_rtde] Unable to get data from robot"); - } - } - else - { - mc_rtc::log::info("[mc_rtde] Running simulation only. No connection to real robot"); - robot_ = new URControl(host, config_param); - - /* Set start state values */ - auto & robot = controller.controller().robots().robot(robot_name); - robot_->setStartState(robot.stance(), state_); - } - - /* Setup log entries */ - logger_.start(controller.current_controller(), controller.timestep()); - addLogEntryRobotInfo(); - logger_.addLogEntry("delay", [this]() { return delay_; }); - - /* Run QP (every timestep ms) and send result joint commands to the robot */ - controller.init(state_.qIn_); - controller.running = true; - controller.controller().gui()->addElement( - {"RTDE"}, mc_rtc::gui::Button("Stop controller", [&controller]() { controller.running = false; })); - - mc_rtc::log::info("[mc_rtde] interface initialized"); -} - -/* Destructor */ -MCControlRtde::~MCControlRtde() -{ - delete robot_; -} - -void MCControlRtde::robotControl() -{ - while(globalController_.running) - { - auto start_t_ = std::chrono::high_resolution_clock::now(); - auto robot_name = globalController_.robot().name(); - - if(host_ != "simulation") - { - /* Store the data received by "recvData()" in "state" */ - robot_->getState(state_); - } - - /* Send sensor readings to mc_rtc controller */ - globalController_.setEncoderValues(robot_name, state_.qIn_); - globalController_.setEncoderVelocities(robot_name, state_.dqIn_); - globalController_.setJointTorques(robot_name, state_.torqIn_); - - // Run the controller - if(globalController_.run()) - { - /* Update control value from the data in a robot */ - auto & robot = globalController_.controller().robots().robot(robot_name); - const auto & rjo = robot.refJointOrder(); - for(size_t i = 0; i < UR_JOINT_COUNT; ++i) - { - auto jIndex = robot.jointIndexByName(rjo[i]); - cmdData_.qOut_[i] = robot.mbc().q[jIndex][0]; - cmdData_.dqOut_[i] = robot.mbc().alpha[jIndex][0]; - cmdData_.torqOut_[i] = robot.mbc().jointTorque[jIndex][0]; - } - - if(host_ != "simulation") - { - /* Send command data */ - robot_->sendCmdData(cmdData_); - } - else - { - /* Loop back the value of "cmdData_" to "state_" */ - robot_->loopbackState(cmdData_, state_); - } - } - - /* Wait until next controller run */ - auto now = std::chrono::high_resolution_clock::now(); - double elapsed = std::chrono::duration(now - start_t_).count(); - if(elapsed > globalController_.timestep() * 1000) - { - mc_rtc::log::warning("[mc_rtde] Loop time {} exeeded timestep of {} ms", elapsed, - globalController_.timestep() * 1000); - } - else - { - std::this_thread::sleep_for( - std::chrono::microseconds(static_cast((globalController_.timestep() * 1000 - elapsed)) * 1000)); - } - - /* Print controller data to the log */ - logger_.log(); - delay_ = std::chrono::duration(std::chrono::high_resolution_clock::now() - start_t_).count(); - } - - robot_->controlFinished(); -} - -void MCControlRtde::addLogEntryRobotInfo() -{ - /* Sensors */ - /* Position(Angle) values */ - logger_.addLogEntry("measured_joint_position", [this]() -> const std::vector & { return state_.qIn_; }); - /* Velocity values */ - logger_.addLogEntry("measured_joint_velocity", [this]() -> const std::vector & { return state_.dqIn_; }); - /* Torque values */ - logger_.addLogEntry("measured_joint_torque", [this]() -> const std::vector & { return state_.torqIn_; }); - - /* Command data to send to the robot */ - /* Position(Angle) values */ - logger_.addLogEntry("sent_joint_position", [this]() -> const std::vector & { return cmdData_.qOut_; }); - /* Velocity values */ - logger_.addLogEntry("sent_joint_velocity", [this]() -> const std::vector & { return cmdData_.dqOut_; }); - /* Torque values */ - logger_.addLogEntry("sent_joint_torque", [this]() -> const std::vector & { return cmdData_.torqOut_; }); -} - -} // namespace mc_rtde diff --git a/src/MCControlRtde.h b/src/MCControlRtde.h deleted file mode 100644 index 8ac8f07..0000000 --- a/src/MCControlRtde.h +++ /dev/null @@ -1,52 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "URControl.h" - -namespace mc_rtde -{ -/** - * @brief mc_rtc control interface for rtde robots - */ -struct MCControlRtde -{ - /** - * @brief Interface constructor and destructor - */ - MCControlRtde(mc_control::MCGlobalController & controller, const std::string & host); - - virtual ~MCControlRtde(); - - /** - * @brief Return a reference to the global mc_rtc controller - */ - mc_control::MCGlobalController & controller() - { - return globalController_; - } - - /** - * @brief Run of the robot control loop - */ - void robotControl(); - -private: - void addLogEntryRobotInfo(); - - /*! Global mc_rtc controller */ - mc_control::MCGlobalController & globalController_; - - /*! Connection host */ - std::string host_; - - URControl * robot_; - URSensorInfo state_; - URCommandData cmdData_; - mc_rtc::Logger logger_; - double delay_; -}; - -} // namespace mc_rtde diff --git a/src/URControl.cpp b/src/URControl.cpp index 782df38..e23dd2a 100644 --- a/src/URControl.cpp +++ b/src/URControl.cpp @@ -1,180 +1,260 @@ #include "URControl.h" +#include "ControlMode.h" +#include +#include +#include +#include #include +#include +#include + +#include +namespace po = boost::program_options; namespace mc_rtde { -/** - * @brief Interface constructor and destructor - * UDP connection with robot using specified parameters. - * Control level is set to LOW-level. - * - * @param config_param Configuration file parameters - */ -URControl::URControl(const URConfigParameter & config_param) -: ur_rtde_control_(nullptr), ur_rtde_receive_(nullptr), cm_(config_param.cm_), host_(""), - joint_speed_(config_param.joint_speed_), joint_acceleration_(config_param.joint_acceleration_) +struct ControlLoopDataBase { - try + + ControlLoopDataBase(ControlMode cm) : cm_(cm), controller_(nullptr), ur_threads_(nullptr){}; + + ControlMode cm_; + mc_control::MCGlobalController * controller_; + std::thread * controller_run_; + std::condition_variable controller_run_cv_; + std::vector * ur_threads_; +}; + +template +struct ControlLoopData : public ControlLoopDataBase +{ + ControlLoopData() : ControlLoopDataBase(cm), urs(nullptr){}; + + std::vector> * urs; +}; + +template +void * global_thread_init(mc_control::MCGlobalController::GlobalConfiguration & qconfig) +{ + auto rtdeConfig = qconfig.config("RTDE"); + + auto loop_data = new ControlLoopData(); + loop_data->controller_ = new mc_control::MCGlobalController(qconfig); + loop_data->ur_threads_ = new std::vector(); + auto & controller = *loop_data->controller_; + + if(controller.controller().timeStep < 0.002) { - ur_rtde_control_ = new ur_rtde::RTDEControlInterface(config_param.targetIP_); - ur_rtde_receive_ = new ur_rtde::RTDEReceiveInterface(config_param.targetIP_); + mc_rtc::log::error_and_throw("[mc_rtde] mc_rtc cannot run faster than 2kHz with mc_rtde"); } - catch(const std::exception & e) + + size_t n_steps = std::ceil(controller.controller().timeStep / 0.001); + size_t freq = std::ceil(1 / controller.controller().timeStep); + mc_rtc::log::info("[mc_rtde] mc_rtc running at {}Hz, will interpolate every {} ur control step", freq, n_steps); + auto & robots = controller.controller().robots(); + // Initialize all real robots + for(size_t i = controller.realRobots().size(); i < robots.size(); ++i) { - mc_rtc::log::error_and_throw("[mc_rtde] Could not connect to ur robot: {}", e.what()); + controller.realRobots().robotCopy(robots.robot(i), robots.robot(i).name()); } -} + // Initialize controlled ur robots + loop_data->urs = new std::vector>(); + auto & urs = *loop_data->urs; -/** - * @brief Interface constructor and destructor - * Running simulation only. No connection to real robot. - * - * @param host "simulation" only - * @param config_param Configuration file parameters - */ -URControl::URControl(const std::string & host, const URConfigParameter & config_param) -: ur_rtde_control_(nullptr), ur_rtde_receive_(nullptr), cm_(config_param.cm_), host_(host), - joint_speed_(config_param.joint_speed_), joint_acceleration_(config_param.joint_acceleration_) -{ -} - -/** - * @brief Save the data received from the robot - * - * @param state Current sensor values information - * @return true Success - * @return false Could not receive - */ -bool URControl::getState(URSensorInfo & state) -{ - try + std::vector ur_init_thread; + std::mutex ur_init_mutex; + std::condition_variable ur_init_cv; + bool ur_init_ready = false; + for(auto & robot : robots) { - /* Save the data received from the robot in "state" */ - state.qIn_ = ur_rtde_receive_->getActualQ(); - state.dqIn_ = ur_rtde_receive_->getActualQd(); - // state.torqIn_ = ur_rtde_receive_->; - if(state.qIn_.empty() || state.dqIn_.empty()) + if(robot.mb().nrDof() == 0) { - mc_rtc::log::error("[mc_rtde] Data could not be received"); - return false; + continue; + } + + if(rtdeConfig.has(robot.name())) + { + std::string ip = rtdeConfig(robot.name())("ip"); + ur_init_thread.emplace_back( + [&, ip]() + { + { + std::unique_lock lock(ur_init_mutex); + ur_init_cv.wait(lock, [&ur_init_ready]() { return ur_init_ready; }); + } + + auto ur = std::unique_ptr>(new URControlLoop(robot.name(), ip)); + std::unique_lock lock(ur_init_mutex); + }); + } + else + { + mc_rtc::log::warning("The loaded controller uses an actuated robot that is not configured and not ignored: {}", + robot.name()); } } - catch(const std::exception & e) - { - mc_rtc::log::error("[mc_rtde] Data could not be received due to an error in the ur_rtde library: {}", e.what()); - return false; - } - return true; -} -/** - * @brief Set start state values for simulation - * - * @param stance Value defined by RobotModule - * @param state Current sensor values information - */ -void URControl::setStartState(const std::map> & stance, URSensorInfo & state) -{ - /* Start stance */ - for(int i = 0; i < UR_JOINT_COUNT; ++i) + ur_init_ready = true; + ur_init_cv.notify_all(); + for(auto & th : ur_init_thread) { - state.qIn_[i] = 0.0; - state.dqIn_[i] = 0.0; - state.torqIn_[i] = 0.0; + th.join(); } - /* Set position(Angle) values */ - try + for(auto & ur : urs) { - state.qIn_[0] = stance.at("shoulder_pan_joint")[0]; - state.qIn_[1] = stance.at("shoulder_lift_joint")[0]; - state.qIn_[2] = stance.at("elbow_joint")[0]; - state.qIn_[3] = stance.at("wrist_1_joint")[0]; - state.qIn_[4] = stance.at("wrist_2_joint")[0]; - state.qIn_[5] = stance.at("wrist_3_joint")[0]; + ur.init(controller); } - catch(const std::exception & e) + + controller.init(robots.robot().encoderValues()); + controller.running = true; + controller.controller().gui()->addElement( + {"RTDE"}, mc_rtc::gui::Button("Stop controller", [&controller]() { controller.running = false; })); + + // Start ur control loop + static std::mutex startMutex; + static std::condition_variable startCV; + static bool startControl = false; + + for(auto & ur : urs) { - mc_rtc::log::error("[mc_rtde] Failed to get the value defined by RobotModule: {}", e.what()); + loop_data->ur_threads_->emplace_back( + [&]() { ur.controlThread(controller, startMutex, startCV, startControl, controller.running); }); } - /* copy to private member state */ - stateIn_ = state; -}; + startControl = true; + startCV.notify_all(); + + loop_data->controller_run_ = new std::thread( + [loop_data]() + { + auto controller_ptr = loop_data->controller_; + auto & controller = *controller_ptr; + auto & urs_ = *loop_data->urs; + std::mutex controller_run_mtx; + std::timespec tv; + clock_gettime(CLOCK_REALTIME, &tv); + // Current time in milliseconds + double current_t = tv.tv_sec * 1000 + tv.tv_nsec * 1e-6; + // Will record the time that passed between two runs + double elapsed_t = 0; + controller.controller().logger().addLogEntry("mc_franka_delay", [&elapsed_t]() { return elapsed_t; }); + while(controller.running) + { + std::unique_lock lck(controller_run_mtx); + loop_data->controller_run_cv_.wait(lck); + clock_gettime(CLOCK_REALTIME, &tv); + elapsed_t = tv.tv_sec * 1000 + tv.tv_nsec * 1e-6 - current_t; + current_t = elapsed_t + current_t; + + // Update from ur sensors + for(auto & ur : urs_) + { + ur.updateSensors(controller); + } + + // Run the controller + controller.run(); -/** - * @brief Loop back the value of "data" to "state" - * - * @param data Command data for sending to UR5e robot - * @param state Current sensor values information - */ -void URControl::loopbackState(const URCommandData & data, URSensorInfo & state) + // Update ur commands + for(auto & ur : urs_) + { + ur.updateControl(controller); + } + } + }); + + return loop_data; +} + +template +void run_impl(void * data) { - /* Set current sensor values */ - for(int i = 0; i < UR_JOINT_COUNT; ++i) + auto control_data = static_cast *>(data); + auto controller_ptr = control_data->controller_; + auto & controller = *controller_ptr; + while(controller.running) + { + control_data->controller_run_cv_.notify_one(); + // Sleep until the next cycle + sched_yield(); + } + for(auto & th : *control_data->ur_threads_) { - state.qIn_[i] = data.qOut_[i]; - state.dqIn_[i] = data.dqOut_[i]; - state.torqIn_[i] = data.torqOut_[i]; + th.join(); } + control_data->controller_run_->join(); + delete control_data->urs; + delete controller_ptr; } -/** - * @brief Send control commands to the robot - * - * @param sendData Command data for sending to UR5e robot - */ -void URControl::sendCmdData(URCommandData & sendData) +void run(void * data) { - const double speed_acceleration = 0.5; - const bool asynchronous = true; - const std::vector task_frame = {0, 0, 0, 0, 0, 0}; - const std::vector selection_vector = {1, 1, 1, 1, 1, 1}; - const int force_type = 2; - const std::vector limits = {2, 2, 1.5, 1, 1, 1}; - bool ret; - - if(host_ != "simulation") + auto control_data = static_cast(data); + switch(control_data->cm_) { - try - { - /* Send command data */ - ret = false; - switch(cm_) - { - case mc_rtde::ControlMode::Position: - ret = ur_rtde_control_->moveJ(sendData.qOut_, joint_speed_, joint_acceleration_, asynchronous); - break; - case mc_rtde::ControlMode::Velocity: - ret = ur_rtde_control_->speedJ(sendData.dqOut_, speed_acceleration); - break; - case mc_rtde::ControlMode::Torque: - ret = ur_rtde_control_->forceMode(task_frame, selection_vector, sendData.torqOut_, force_type, limits); - break; - } - if(!ret) - { - mc_rtc::log::error("[mc_rtde] Could not send command to UR5e robot"); - } - } - catch(const std::exception & e) - { - mc_rtc::log::error("[mc_rtde] The command could not be sent due to an error in the ur_rtde library: {}", - e.what()); - } + case ControlMode::Position: + run_impl(data); + break; + case ControlMode::Velocity: + run_impl(data); + break; + case ControlMode::Torque: + run_impl(data); + break; } } -/** - * @brief The control of the robot is finished - */ -void URControl::controlFinished() +void * init(int argc, char * argv[], uint64_t & cycle_ns) { - if(host_ != "simulation") + std::string conf_file = ""; + po::options_description desc("MCControlRTDE options"); + // clang-format off + desc.add_options() + ("help", "Display help message") + ("conf,f", po::value(&conf_file), "Configuration file"); + // clang-format on + + po::variables_map vm; + po::store(po::parse_command_line(argc, argv, desc), vm); + po::notify(vm); + + if(vm.count("help")) + { + std::cout << desc << "\n"; + std::cout << "see etc/mc_rtc_ur.yaml for ur_rtde configuration\n"; + return nullptr; + } + + mc_control::MCGlobalController::GlobalConfiguration gconfig(conf_file, nullptr); + if(!gconfig.config.has("Franka")) + { + mc_rtc::log::error_and_throw( + "No Franka section in the configuration, see etc/sample.yaml for an example"); + } + auto frankaConfig = gconfig.config("Franka"); + ControlMode cm = frankaConfig("ControlMode", ControlMode::Velocity); + bool ShowNetworkWarnings = frankaConfig("ShowNetworkWarnings", true); + try + { + switch(cm) + { + case ControlMode::Position: + return global_thread_init(gconfig); + case ControlMode::Velocity: + return global_thread_init(gconfig); + case ControlMode::Torque: + return global_thread_init(gconfig); + default: + return nullptr; + } + } + catch(const std::exception & e) { - ur_rtde_control_->stopScript(); + std::cerr << "mc_rtde::Exception " << e.what() << "\n"; + return nullptr; } } diff --git a/src/URControl.h b/src/URControl.h index e9eab6e..0a4a459 100644 --- a/src/URControl.h +++ b/src/URControl.h @@ -1,165 +1,141 @@ #pragma once -#include -#include -#include -#include -#include +#include + +#include #include #include #include "ControlMode.h" +#include "URControlType.h" namespace mc_rtde { const std::vector ROBOT_NAMES = {"ur5e", "ur10"}; const std::string CONFIGURATION_FILE = "/usr/local/etc/mc_rtde/mc_rtc_ur.yaml"; -constexpr int UR_JOINT_COUNT = 6; -/** - * @brief Configuration file parameters for mc_rtde - */ -struct URConfigParameter +template +struct URControlLoop { - URConfigParameter() : cm_(ControlMode::Position), joint_speed_(1.05), joint_acceleration_(1.4), targetIP_("localhost") - { - } + URControlLoop(const std::string & name, const std::string & ip); + + void init(mc_control::MCGlobalController & controller); + + void updateSensors(mc_control::MCGlobalController & controller); + + void updateControl(mc_control::MCGlobalController & controller); + + void controlThread(mc_control::MCGlobalController & controller, + std::mutex & startM, + std::condition_variable & startCV, + bool & start, + bool & running); + +private: + std::string name_; + mc_rtc::Logger logger_; + size_t sensor_id_ = 0; + rbd::MultiBodyConfig command_; + size_t control_id_ = 0; + size_t prev_control_id_ = 0; + double delay_ = 0; + + URSensorInfo state_; + URControlType control_; + + uint16_t flags = ur_rtde::RTDEControlInterface::FLAG_VERBOSE | ur_rtde::RTDEControlInterface::FLAG_UPLOAD_SCRIPT; + /* Communication information with a real robot */ - /* ControlMode (position, velocity or torque) */ - ControlMode cm_; - /* joint speed [rad/s] (Valid only when ControlMode is Position) */ - double joint_speed_; - /* joint acceleration [rad/s^2] (Valid only when ControlMode is Position) */ - double joint_acceleration_; - /* Connection target IP */ - std::string targetIP_; + ur_rtde::RTDEControlInterface * ur_rtde_control_; + ur_rtde::RTDEReceiveInterface * ur_rtde_receive_; + + mutable std::mutex updateSensorsMutex_; + mutable std::mutex updateControlMutex_; + + std::vector sensorsBuffer_ = std::vector(6, 0.0); }; -/** - * @brief Current sensor values information of UR5e robot - */ -struct URSensorInfo +template +using URControlLoopPtr = std::unique_ptr>; + +template +URControlLoop::URControlLoop(const std::string & name, const std::string & ip) +: name_(name), logger_(mc_rtc::Logger::Policy::THREADED, "/tmp", "mc-rtde-" + name_), control_(state_) { - URSensorInfo() - { - qIn_.resize(UR_JOINT_COUNT); - dqIn_.resize(UR_JOINT_COUNT); - torqIn_.resize(UR_JOINT_COUNT); - } - /* Position(Angle) values */ - std::vector qIn_; - /* Velocity values */ - std::vector dqIn_; - /* Torque values */ - std::vector torqIn_; -}; + ur_rtde_control_ = new ur_rtde::RTDEControlInterface(ip, 500, flags, 50002, 85); + ur_rtde_receive_ = new ur_rtde::RTDEReceiveInterface(ip, 500, {}, false, false, 90); +} -/** - * @brief Command data for sending to UR5e robot - */ -struct URCommandData +template +void URControlLoop::init(mc_control::MCGlobalController & controller) { - URCommandData() + logger_.start(controller.current_controller(), 0.002); + logger_.addLogEntry("sensor_id", [this] { return sensor_id_; }); + logger_.addLogEntry("prev_control_id", [this]() { return prev_control_id_; }); + logger_.addLogEntry("control_id", [this]() { return control_id_; }); + logger_.addLogEntry("delay", [this]() { return delay_; }); + updateSensors(controller); + updateControl(controller); + + auto & robot = controller.controller().robots().robot(name_); + auto & real = controller.controller().realRobots().robot(name_); + const auto & rjo = robot.refJointOrder(); + for(size_t i = 0; i < rjo.size(); ++i) { - qOut_.resize(UR_JOINT_COUNT); - dqOut_.resize(UR_JOINT_COUNT); - torqOut_.resize(UR_JOINT_COUNT); - for(int i = 0; i < UR_JOINT_COUNT; ++i) - { - qOut_[i] = 0.0; - dqOut_[i] = 0.0; - torqOut_[i] = 0.0; - } + auto jIndex = robot.jointIndexByName(rjo[i]); + robot.mbc().q[jIndex][0] = ur_rtde_receive_->getActualQ()[i]; + robot.mbc().jointTorque[jIndex][0] = ur_rtde_control_->getJointTorques()[i]; } - /* Position(Angle) values */ - std::vector qOut_; - /* Velocity values */ - std::vector dqOut_; - /* Torque values */ - std::vector torqOut_; -}; + robot.forwardKinematics(); + real.mbc() = robot.mbc(); +} -/** - * @brief mc_rtc control interface for UR5e robot - */ -class URControl +template +void URControlLoop::updateSensors(mc_control::MCGlobalController & controller) { - /* Communication information with a real robot */ - ur_rtde::RTDEControlInterface * ur_rtde_control_; - ur_rtde::RTDEReceiveInterface * ur_rtde_receive_; - - /* ControlMode (position, velocity or torque) */ - ControlMode cm_; - /* joint speed [rad/s] (Valid only when ControlMode is Position) */ - double joint_speed_; - /* joint acceleration [rad/s^2] (Valid only when ControlMode is Position) */ - double joint_acceleration_; - /* Connection host, robot name or "simulation" */ - std::string host_; - /* Current sensor values information */ - URSensorInfo stateIn_; - -public: - /** - * @brief Interface constructor and destructor - * ur_rtde connection with robot using specified parameters. - * - * @param config_param Configuration file parameters - */ - URControl(const URConfigParameter & config_param); - - /** - * @brief Interface constructor and destructor - * Running simulation only. No connection to real robot. - * - * @param host "simulation" only - * @param config_param Configuration file parameters - */ - URControl(const std::string & host, const URConfigParameter & config_param); - - ~URControl() + std::unique_lock lock(updateSensorsMutex_); + auto & robot = controller.robots().robot(name_); + using GC = mc_control::MCGlobalController; + using set_sensor_t = void (GC::*)(const std::string &, const std::vector &); + auto updateSensor = [&controller, &robot, this](set_sensor_t set_sensor, const std::vector & data) { - delete ur_rtde_control_; - delete ur_rtde_receive_; + assert(sensorsBuffer_.size() == 6); + std::memcpy(sensorsBuffer_.data(), data.data(), 6 * sizeof(double)); + (controller.*set_sensor)(robot.name(), sensorsBuffer_); }; - /** - * @brief Save the data received from the robot - * - * @param state Current sensor values information - * @return true Success - * @return false Could not receive - */ - bool getState(URSensorInfo & state); - - /** - * @brief Set the start state values for simulation - * - * @param stance Value defined by RobotModule - * @param state Current sensor values information - */ - void setStartState(const std::map> & stance, URSensorInfo & state); - - /** - * @brief Loop back the value of "data" to "stateIn" - * - * @param data Command data for sending to UR5e robot - * @param state Current sensor values information - */ - void loopbackState(const URCommandData & data, URSensorInfo & state); - - /** - * @brief Send control commands to the robot - * - * @param data Command data for sending to UR5e robot - */ - void sendCmdData(URCommandData & data); - - /** - * @brief The control of the robot is finished - */ - void controlFinished(); -}; + updateSensor(&GC::setEncoderValues, state_.qIn_); + updateSensor(&GC::setEncoderVelocities, state_.dqIn_); + updateSensor(&GC::setJointTorques, state_.torqIn_); +} + +template +void URControlLoop::updateControl(mc_control::MCGlobalController & controller) +{ + std::unique_lock lock(updateControlMutex_); + auto & robot = controller.robots().robot(name_); + command_ = robot.mbc(); + control_id_++; +} + +template +void URControlLoop::controlThread(mc_control::MCGlobalController & controller, + std::mutex & startM, + std::condition_variable & startCV, + bool & start, + bool & running) +{ + { + std::unique_lock lock(startM); + startCV.wait(lock, [&]() { return start; }); + } + + std::unique_lock ctlLock(updateControlMutex_); + std::unique_lock senLock(updateSensorsMutex_); + + control_.update(state_); + control_.control(*ur_rtde_control_); +} } // namespace mc_rtde diff --git a/src/URControlType.h b/src/URControlType.h new file mode 100644 index 0000000..bd3746e --- /dev/null +++ b/src/URControlType.h @@ -0,0 +1,114 @@ +#pragma once + +#include +#include + +#include "ControlMode.h" + +namespace mc_rtde +{ + +/** + * @brief Current sensor values information of UR5e robot + */ +struct URSensorInfo +{ + URSensorInfo() + { + qIn_.resize(6); + dqIn_.resize(6); + torqIn_.resize(6); + } + /* Position(Angle) values */ + std::vector qIn_; + /* Velocity values */ + std::vector dqIn_; + /* Torque values */ + std::vector torqIn_; +}; + +template +struct URControlType +{ + static_assert(static_cast(cm) == static_cast(cm) + 1, "This must be specialized"); +}; + +template<> +struct URControlType +{ +private: + // Parameters + const double dt = 0.002; + const double lookahead_time = 0.001; + const double acceleration = 0.5; + const double velocity = 0.5; + const double gain = 300; + + URSensorInfo state_; + +public: + URControlType(const URSensorInfo & state) : state_(state){}; + + void update(const URSensorInfo & state) + { + state_ = state; + } + + void control(ur_rtde::RTDEControlInterface & robot_) + { + auto start_t = robot_.initPeriod(); + robot_.servoJ(state_.qIn_, velocity, acceleration, dt, lookahead_time, gain); + robot_.waitPeriod(start_t); + } +}; + +template<> +struct URControlType +{ +private: + double acceleration = 0.5; + double dt = 0.002; + + URSensorInfo state_; + +public: + URControlType(const URSensorInfo & state) : state_(state){}; + + void update(const URSensorInfo & state) + { + state_ = state; + } + + void control(ur_rtde::RTDEControlInterface & robot_) + { + auto start_t = robot_.initPeriod(); + robot_.speedJ(state_.dqIn_, acceleration, dt); + robot_.waitPeriod(start_t); + } +}; + +template<> +struct URControlType +{ +private: + double acceleration = 0.5; + double dt = 0.002; + + URSensorInfo state_; + +public: + URControlType(const URSensorInfo & state) : state_(state){}; + + void update(const URSensorInfo & state) + { + state_ = state; + } + + void control(ur_rtde::RTDEControlInterface & robot_) + { + auto start_t = robot_.initPeriod(); + // TODO check the documentation + robot_.waitPeriod(start_t); + } +}; +} // namespace mc_rtde diff --git a/src/main.cpp b/src/main.cpp index d1efd74..a6b4ccb 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,86 +1,76 @@ -#include "MCControlRtde.h" - -#include - -#include -#include - -namespace po = boost::program_options; - -namespace +/* Copyright 2020 mc_rtc development team */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "thread.h" + +int sched_setattr(pid_t pid, const struct sched_attr * attr, unsigned int flags) { - -/* Checking the existence of the file */ -/* Return value: true if the file exists, false otherwise */ -bool file_exists(const std::string & str) -{ - std::ifstream fs(str); - return fs.is_open(); + return syscall(__NR_sched_setattr, pid, attr, flags); } -} // namespace - -/* Main function of the interface */ int main(int argc, char * argv[]) { - /* Set command line arguments options */ - /* Usage example: MCControlRtde -h simulation -f @ETC_PATH@/mc_rtde/mc_rtc_xxxxx.yaml */ - std::string conf_file; - std::string host; - po::options_description desc(std::string("MCControlRtde options")); + struct sched_attr attr; - // Get the configuration file path dedicated to this program - std::string check_file = mc_rtde::CONFIGURATION_FILE; - if(!file_exists(check_file)) + /* Lock memory */ + if(mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { - check_file = ""; + printf("mlockall failed: %m\n"); + if(errno == ENOMEM) + { + printf("\nIt is likely your user does not have enough memory limits, you can change the limits by adding the " + "following line to /etc/security/limits.conf:\n\n"); + printf("%s - memlock 1000000000\n\n", getlogin()); + printf("Then log-in and log-out\n"); + } + return -2; } - // clang-format off - desc.add_options() - ("help", "display help message") - ("host,h", po::value(&host)->default_value("ur5e"), "connection host, robot name {ur5e, ur10} or \"simulation\"") - ("conf,f", po::value(&conf_file)->default_value(check_file), "configuration file"); - // clang-format on + /* Configure deadline policy */ + memset(&attr, 0, sizeof(attr)); + attr.size = sizeof(attr); - /* Parse command line arguments */ - po::variables_map vm; - try + uint64_t cycle_ns = 1 * 1000 * 1000; // 1 ms default cycle + char * MC_RT_FREQ = nullptr; + if((MC_RT_FREQ = getenv("MC_RT_FREQ")) != nullptr) { - po::store(po::parse_command_line(argc, argv, desc), vm); + cycle_ns = atoi(MC_RT_FREQ) * 1000 * 1000; } - catch(const std::exception & e) - { - std::cerr << e.what() << '\n'; - return 1; - } - po::notify(vm); - if(vm.count("help")) + + /* Initialize callback (non real-time yet) */ + void * data = mc_rtde::init(argc, argv, cycle_ns); + if(!data) { - std::cout << desc << std::endl; - return 1; + printf("Initialization failed\n"); + return -2; } - mc_rtc::log::info("[mc_rtde] Reading additional configuration from {}", conf_file); - /* Create global controller */ - mc_control::MCGlobalController gconfig(conf_file, nullptr); + /* Time reservation */ + attr.sched_policy = SCHED_DEADLINE; + attr.sched_runtime = attr.sched_deadline = attr.sched_period = cycle_ns; // nanoseconds - /* Check that the interface can work with the main controller robot */ - if(std::count(mc_rtde::ROBOT_NAMES.begin(), mc_rtde::ROBOT_NAMES.end(), gconfig.robot().name()) == 0) + printf("Running real-time thread at %fms per cycle\n", cycle_ns / 1e6); + + /* Set scheduler policy */ + if(sched_setattr(0, &attr, 0) < 0) { - std::string s; - for(const auto & r : mc_rtde::ROBOT_NAMES) s += r; - mc_rtc::log::error("[mc_rtde] This program can only handle {} at the moment", s); - return 1; + printf("sched_setattr failed: %m\n"); + return -2; } - /* Create MCControlRtde interface */ - mc_rtde::MCControlRtde mc_control_rtde(gconfig, host); - - /* Run of the robot control loop */ - mc_control_rtde.robotControl(); - - mc_rtc::log::info("[mc_rtde] Terminated"); + /* Run */ + mc_rtde::run(data); return 0; } diff --git a/src/thread.h b/src/thread.h new file mode 100644 index 0000000..86c8c80 --- /dev/null +++ b/src/thread.h @@ -0,0 +1,14 @@ +#pragma once + +#include + +namespace mc_rtde +{ + +// Called in non real-time context to initialize the application +void * init(int argc, char * argv[], uint64_t & cycle_ns); + +// Main control-loop, when waiting for the next loop one should simply call sched_yield() +void run(void * data); + +} // namespace mc_rtde