Adding SURENA - MiladShafiee/HLLP GitHub Wiki

In the name of God
A sample could be generated in the choreonoid by creating a folder in the "sample" folder. We called this folder "SURENA". SURENA should contain a CMakeLists.txt in which sample properties comes. Following is the SURENA CMakeLists.txt,

option(BUILD_SURENA "Building a sample plugin \"SURENA\"" OFF)
configure_file(SR1Minimum.cnoid ${CNOID_SOURCE_SHARE_DIR}/project COPYONLY)

set(sources
  SURENA.cpp
  Mine.cpp
  Robot.cpp
  LinkM.cpp
  Taskspace.cpp
)

set(headers
  Mine.h
  Robot.h
  LinkM.h
  Taskspace.h
)


if(BUILD_SURENA)
  set(target SURENA)
  add_cnoid_simple_controller(${target} SURENA ${sources} ${headers})

  if(QT5)
    qt5_use_modules(${target} Core)
  endif()

endif()

target_link_libraries(${target} SURENA ${QT_LIBRARIES} CnoidBodyPlugin)
apply_common_setting_for_plugin(${target} "${headers}")

install_sample_source(sources)

To add new classes to this project, the class .cpp and .h file names should be added to the sources and headers in this file. Also, if the class uses a special module of the Qt, the module should be mentioned in the qt5_use_modules. In this sample, we wanted to test our controller on the robot, therefore we used the simple controller plugin to build our project. Meanwhile, we defined "SURENA" class in the SURENA.cpp which inherits from the SimpleController class. This class should overload the "initialize" and "control" methods. The structure of the our class is as follow,

/**
   @author Shin'ichiro Nakaoka
*/

#include <cnoid/SimpleController>
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include "Robot.h"

using namespace cnoid;
using namespace std;

const double pgain[] = {
    8000.0, 8000.0, 8000.0, 8000.0, 8000.0, 8000.0,
    3000.0, 3000.0, 3000.0, 3000.0, 3000.0, 3000.0, 3000.0,
    8000.0, 8000.0, 8000.0, 8000.0,
    8000.0, 8000.0,
    3000.0, 3000.0, 3000.0, 3000.0, 3000.0, 3000.0, 3000.0,
    8000.0, 8000.0, 8000.0 };

const double dgain[] = {
    100.0, 100.0, 100.0, 100.0, 100.0, 100.0,
    100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0,
    100.0, 100.0, 100.0, 100.0, 100.0, 100.0,
    100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0,
    100.0, 100.0, 100.0 };


class SURENA : public SimpleController
{
    BodyPtr ioBody;
    Robot temp;
    QList<LinkM> links;
    vector<double> qref;
    vector<double> qold;
    int NStep;
    int var;
    double dt;

public:

    virtual bool initialize(SimpleControllerIO* io) override
    {
        ioBody = io->body();
        dt = io->timeStep();
        VectorXd x;
        x.setLinSpaced(81,0,0.4);
        NStep = x.rows();
        var = 0;

        for(auto joint : ioBody->joints()){
            joint->setActuationMode(Link::JOINT_TORQUE);
            io->enableIO(joint);
            qref.push_back(joint->q());
        }
        qold = qref;

        return true;
    }

    virtual bool control() override
    {
        if(var < NStep)
            var++;
        temp.doIK(var,"RLEG_J6");
        links = temp.GetLinks();
        for(int  i = 1;i < links.size();i++)
        {

            qref[i-1] = links[i].JointAngle;
            cout << qref[i] <<" , "<<flush;
        }
        cout << endl<<flush;

        for(int i=0; i < ioBody->numJoints(); ++i){
            Link* joint = ioBody->joint(i);
            double u;
            double q = joint->q();
            double dq = (q - qold[i]) / dt;
            u = (qref[i] - q) * pgain[i] + (0.0 - dq) * dgain[i];
            qold[i] = q;
            joint->u() = u;
        }
        return true;
    }
};

CNOID_IMPLEMENT_SIMPLE_CONTROLLER_FACTORY(SURENA)
⚠️ **GitHub.com Fallback** ⚠️