xbot2 plugin optionally built

CMakeLists.txt

@@ -333,7 +333,11 @@ add_subdirectory(bindings/python/)
 include(GenerateDeb)
 
 
-option(CARTESIO_COMPILE_XBOT_PLUGIN "Build plugins for XBotCore" OFF)
+option(CARTESIO_COMPILE_XBOT2_PLUGIN "Build plugins for XBot2" OFF)
+
+if(CARTESIO_COMPILE_XBOT2_PLUGIN)
+    add_subdirectory(src/xbot2)
+endif()
 
 # Mark cpp header files for installation
 install(DIRECTORY include/cartesian_interface/

src/xbot2/CMakeLists.txt

@@ -0,0 +1,8 @@
+find_package(xbot2 REQUIRED)
+
+add_xbot2_plugin(cartesio_plugin cartesioplugin.cpp)
+
+target_link_libraries(cartesio_plugin PUBLIC CartesianInterface)
+
+install(TARGETS cartesio_plugin
+    DESTINATION lib)

src/xbot2/cartesioplugin.cpp

@@ -0,0 +1,217 @@
+#include "cartesioplugin.h"
+
+using namespace XBot;
+using namespace XBot::Cartesian;
+
+bool CartesioRt::on_initialize()
+{
+    setJournalLevel(Journal::Level::Low);
+
+    _nh = std::make_unique<ros::NodeHandle>();
+
+    /* Get ik problem from ros param */
+    auto problem_param = getParamOr<std::string>("~problem_param",
+                                                 "cartesian/problem_description");
+    std::string ik_str;
+    if(getParam("~problem_description/content", ik_str))
+    {
+
+    }
+    else if(!_nh->getParam(problem_param, ik_str))
+    {
+        jerror("ros param '{}' not found \n", problem_param);
+        return false;
+    }
+
+    auto ik_yaml = YAML::Load(ik_str);
+
+    /* Create model and ci for rt loop */
+    _rt_model = _robot->model().clone();
+
+    auto rt_ctx = std::make_shared<Cartesian::Context>(
+        std::make_shared<Parameters>(getPeriodSec()),
+        _rt_model);
+
+    ProblemDescription ik_problem(ik_yaml, rt_ctx);
+
+    auto impl_name = getParamOr<std::string>("~solver", "OpenSot");
+
+    _rt_ci = CartesianInterfaceImpl::MakeInstance(impl_name, ik_problem, rt_ctx);
+    _rt_ci->enableOtg(rt_ctx->params()->getControlPeriod());
+    _rt_ci->update(0, 0);
+
+    /* Create model and ci for nrt loop */
+    ModelInterface::Ptr nrt_model = ModelInterface::getModel(_robot->getConfigOptions());
+
+    auto nrt_ctx = std::make_shared<Cartesian::Context>(
+        std::make_shared<Parameters>(*_rt_ci->getContext()->params()),
+        nrt_model);
+
+    _nrt_ci = std::make_shared<LockfreeBufferImpl>(_rt_ci.get(), nrt_ctx);
+    _nrt_ci->pushState(_rt_ci.get(), _rt_model.get());
+    _nrt_ci->updateState();
+    auto nrt_ci = _nrt_ci;
+
+    /*  Create ros api server */
+    RosServerClass::Options opt;
+    opt.tf_prefix = getParamOr<std::string>("~tf_prefix", "ci");
+    opt.ros_namespace = getParamOr<std::string>("~ros_ns", "cartesian");
+    auto ros_srv = std::make_shared<RosServerClass>(_nrt_ci, opt);
+
+    /* Initialization */
+    _rt_active = false;
+    auto rt_active_ptr = &_rt_active;
+
+    _nrt_exit = false;
+    auto nrt_exit_ptr = &_nrt_exit;
+
+    _qdot = _q.setZero(_rt_model->getNv());
+
+    /* Spawn thread */
+    _nrt_th = std::make_unique<thread>(
+        [rt_active_ptr, nrt_exit_ptr, nrt_ci, ros_srv]()
+        {
+            this_thread::set_name("cartesio_nrt");
+
+            while(!*nrt_exit_ptr)
+            {
+                this_thread::sleep_for(10ms);
+
+                if(!*rt_active_ptr) continue;
+
+                nrt_ci->updateState();
+                ros_srv->run();
+
+            }
+
+        });
+
+    /* Set robot control mode */
+    // setDefaultControlMode(ControlMode::Position() + ControlMode::Effort());
+    setDefaultControlMode(ControlMode::Position());
+
+    /* Feedback */
+    _enable_feedback = getParamOr("~enable_feedback", false);
+
+    if(_enable_feedback)
+    {
+        jinfo("running with feedback enabled \n");
+    }
+
+    /* Link state (ground truth) */
+    auto lss_vec = _robot->getDevices<Hal::LinkStateSensor>().get_device_vector();
+    for(auto d : lss_vec)
+    {
+        if(d->getLinkName() == "base_link")
+        {
+            _lss = d;
+            jinfo("found ground truth pose and twist for base_link");
+        }
+    }
+
+    return true;
+}
+
+void CartesioRt::starting()
+{
+    // we use a fake time, and integrate it by the expected dt
+    _fake_time = 0;
+
+    // align model to current position reference
+    _robot->sense(false);
+    _rt_model->setJointPosition(_robot->getMotorPosition());
+    _rt_model->update();
+
+    if(_lss)
+    {
+        _lss->sense();
+        _rt_model->setFloatingBasePose(_lss->getPose());
+        _rt_model->setFloatingBaseTwist(_lss->getTwist());
+        _rt_model->update();
+    }
+
+    // reset ci
+    _rt_ci->reset(_fake_time);
+
+    // signal nrt thread that rt is active
+    _rt_active = true;
+
+    // transit to run
+    start_completed();
+
+}
+
+void CartesioRt::run()
+{
+    /* Receive commands from nrt */
+    _nrt_ci->callAvailable(_rt_ci.get());
+
+    /* Update robot */
+    if(_enable_feedback)
+    {
+        _robot->sense(false);
+        _rt_model->syncFrom(*_robot);
+
+        if(_lss)
+        {
+            _lss->sense();
+            _rt_model->setFloatingBasePose(_lss->getPose());
+            _rt_model->setFloatingBaseTwist(_lss->getTwist());
+            _rt_model->update();
+        }
+    }
+
+    /* Solve IK */
+    if(!_rt_ci->update(_fake_time, getPeriodSec()))
+    {
+        jerror("unable to solve \n");
+        return;
+    }
+
+    /* Integrate solution */
+    if(!_enable_feedback)
+    {
+        _qdot = _rt_model->getJointVelocity() * getPeriodSec() + _rt_model->getJointAcceleration()*0.5*getPeriodSec()*getPeriodSec();
+        _rt_model->integrateJointPosition(_qdot);
+        _rt_model->update();
+    }
+
+    _fake_time += getPeriodSec();
+
+    /* Send state to nrt */
+    _nrt_ci->pushState(_rt_ci.get(), _rt_model.get());
+
+    /* Move robot */
+    if(_enable_feedback)
+    {
+        _robot->setReferenceFrom(*_rt_model, XBot::ControlMode::EFFORT);
+    }
+    else
+    {
+        _robot->setReferenceFrom(*_rt_model);
+    }
+
+    _robot->move();
+}
+
+void CartesioRt::stopping()
+{
+    _rt_active = false;
+    stop_completed();
+}
+
+void CartesioRt::on_abort()
+{
+    _rt_active = false;
+    _nrt_exit = true;
+}
+
+void CartesioRt::on_close()
+{
+    _nrt_exit = true;
+    jinfo("joining with nrt thread.. \n");
+    if(_nrt_th) _nrt_th->join();
+}
+
+XBOT2_REGISTER_PLUGIN(CartesioRt,
+                      cartesio_plugin);

src/xbot2/cartesioplugin.h

@@ -0,0 +1,49 @@
+#ifndef CARTESIO_RT_H
+#define CARTESIO_RT_H
+
+#include <xbot2/xbot2.h>
+#include <xbot2/gazebo/dev_link_state_sensor.h>
+
+#include <cartesian_interface/CartesianInterfaceImpl.h>
+#include <cartesian_interface/sdk/rt/LockfreeBufferImpl.h>
+#include <cartesian_interface/ros/RosServerClass.h>
+
+namespace XBot {
+
+class CartesioRt : public ControlPlugin
+{
+
+public:
+
+    using ControlPlugin::ControlPlugin;
+
+    bool on_initialize() override;
+    void starting() override;
+    void run() override;
+    void stopping() override;
+    void on_abort() override;
+    void on_close() override;
+
+private:
+
+    std::unique_ptr<ros::NodeHandle> _nh;
+
+    bool _enable_feedback;
+
+    Eigen::VectorXd _q, _qdot;
+    ModelInterface::Ptr _rt_model;
+    Cartesian::CartesianInterfaceImpl::Ptr _rt_ci;
+    Cartesian::LockfreeBufferImpl::Ptr _nrt_ci;
+    std::atomic_bool _rt_active;
+    std::atomic_bool _nrt_exit;
+    double _fake_time;
+
+    std::unique_ptr<thread> _nrt_th;
+
+    Hal::LinkStateSensor::Ptr _lss;
+};
+
+
+}
+
+#endif // CARTESIO_RT_H