search

dqrobotics / cpp-interface-vrep

Vrep interface for the dqrobotics in C++
GNU Lesser General Public License v3.0
3 stars 5 forks source link

This PR adds new functionalities related to the simxCallScriptFunction method of the VREP API #1

Closed juanjqo closed 2 years ago

juanjqo commented 2 years ago

Hi @bvadorno and @mmmarinho,

I added new functionalities related to the simxCallScriptFunction method of the VREP API.

In summary, I added the following methods:

The method call_script_function() returns a custom structure (_call_scriptdata) that encapsulates the returned values of the simxCallScriptFunction (except outBuffer). 

struct call_script_data
{
    int return_code;
    VectorXi output_ints;
    VectorXd output_floats;
    std::vector<std::string> output_strings;
};

Furthermore, for some end-users, it would be great (I think) to have some direct methods to obtain, for instance, the inertial parameters of a robot without the necessity to be fluent in Lua. Because of that, I proposed the methods: extract_inertia_matrix(), extract_center_of_mass() and extract_mass(). The methods extract_inertia_matrix() and extract_center_of_mass() use sim.getShapeInertia since simGetShapeMassAndInertia is deprecated. Furthermore, both methods (extract_center_of mass and extract_inertia_matrix) allow specifying the reference frame: the absolute frame and the shape frame. This functionality was present in simGetShapeMassAndInertia but not in the new sim.getShapeInertia.

The three direct methods proposed in this PR require the DQRoboticsApiCommandServer, which is an object on the CoppeliaSim scene with a child script attached that contains the following functions in Lua:

--(C) Copyright 2022 DQ Robotics Developers

--This file is part of DQ Robotics.
--
--   DQ Robotics is free software: you can redistribute it and/or modify
--    it under the terms of the GNU Lesser General Public License as published by
--    the Free Software Foundation, either version 3 of the License, or
--    (at your option) any later version.
--
--    DQ Robotics is distributed in the hope that it will be useful,
--    but WITHOUT ANY WARRANTY; without even the implied warranty of
--    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
--    GNU Lesser General Public License for more details.
--
--    You should have received a copy of the GNU Lesser General Public License
--    along with DQ Robotics.  If not, see <http://www.gnu.org/licenses/>.

--Contributors:
-- Juan Jose Quiroz Omana -  juanjqo@g.ecc.u-tokyo.ac.jp

----------------------------------------------------------------------------------
--/**
-- * @brief This function is used to test the DQ_VrepInterface::call_script_function.
-- * @param inInts. The integer value of the handle. Example: {1} 
-- * @param inFloats. The float input value. Example: {2.0,3.4} 
-- * @param inStrings. The string input value. Example: {"DQ_Robotics"} 
-- * @param inBuffer. The buffer input value. Example: {}
-- * @returns inInts
-- * @returns inFloats
-- * @returns inStrings
-- * @returns Empty
function test_inputs(inInts,inFloats,inStrings,inBuffer)   
    print('----------------')
    print('test_inputs')
    print(inInts)   
    print(inFloats)   
    print(inStrings) 
    print('----------------')   

    return inInts,inFloats,inStrings,'' 
end
----------------------------------------------------------------------------------

----------------------------------------------------------------------------------
--/**
-- * @brief Returns the mass a handle.
-- * @param inInts. The integer value of the handle. Example: {1} 
-- * @param inFloats. The float input value. Example: {} 
-- * @param inStrings. The string input value. Example: {"absolute_frame"} or {}
-- * @param inBuffer. The buffer input value. Example: {}
-- * @returns the mass
function get_mass(inInts,inFloats,inStrings,inBuffer)
    local mass = {}    
    if #inInts>=1 then    
        mass =sim.getShapeMass(inInts[1]) 
        --print(mass)        
        return {},{mass},{},''              
    end
end
----------------------------------------------------------------------------------

----------------------------------------------------------------------------------
--/**
-- * @brief Returns the inertia of a handle.
-- * @param inInts. The integer value of the handle. Example: {1} 
-- * @param inFloats. The float input value. Example: {} 
-- * @param inStrings. The string input value. Example: {"absolute_frame"} or {}
-- * @param inBuffer. The buffer input value. Example: {}
-- * @returns the inertia
function get_inertia(inInts,inFloats,inStrings,inBuffer)

    if #inInts>=1 then
        IM, matrixSFCOM=sim.getShapeInertia(inInts[1])         

        if inStrings[1] == 'absolute_frame' then
            matrix0_SF=sim.getObjectMatrix(inInts[1],-1) 
            M = sim.multiplyMatrices(matrix0_SF,matrixSFCOM)        
            I= {{IM[1],IM[2],IM[3]},
                {IM[4],IM[5],IM[6]},
                {IM[7],IM[8],IM[9]}}        
            R_0_COM = {{M[1],M[2],M[3]},
                       {M[5],M[6],M[7]},
                       {M[9],M[10],M[11]}}
            R_0_COM_T = transpose(R_0_COM)            
            RIR_T = mat_mult(mat_mult(R_0_COM,I), R_0_COM_T)
            RIR_T_v = {RIR_T[1][1], RIR_T[1][2], RIR_T[1][3],
                       RIR_T[2][1], RIR_T[2][2], RIR_T[2][3],
                       RIR_T[3][1], RIR_T[3][2], RIR_T[3][3]}
            resultInertiaMatrix=RIR_T_v
        else
            resultInertiaMatrix=IM
        end      
        return {},resultInertiaMatrix,{},''              
    end
end
----------------------------------------------------------------------------------

----------------------------------------------------------------------------------
--/**
-- * @brief Returns the center of mass of a handle.
-- * @param inInts. The integer value of the handle. Example: {1} 
-- * @param inFloats. The float input value. Example: {} 
-- * @param inStrings. The string input value. Example: {"absolute_frame"} or {}
-- * @param inBuffer. The buffer input value. Example: {}
-- * @returns the center of mass
function get_center_of_mass(inInts,inFloats,inStrings,inBuffer)
    if #inInts>=1 then
        IM,matrix_SF_COM=sim.getShapeInertia(inInts[1])
        matrix0_SF=sim.getObjectMatrix(inInts[1],-1)       
        if inStrings[1] == 'absolute_frame' then
            resultMatrix = sim.multiplyMatrices(matrix0_SF,matrix_SF_COM)
        else
            resultMatrix = matrix_SF_COM
        end        
        return {},{resultMatrix[4], resultMatrix[8],resultMatrix[12]},{},''              
    end
end
----------------------------------------------------------------------------------

----------------------------------------------------------------------------------
--/**
-- * @brief Returns the tranpose matrix of A.
-- * @param Matrix A. Example A = {{1,2,3},{4,5,6},{7,8,9}} 
-- * @returns the tranpose matrix. Given A, tranpose(A) = {{1,4,7},{2,5,8},{3,6,9}} 
function transpose( A )
    local result = {} 
    for i = 1, #A[1] do
        result[i] = {}
        for j = 1, #A do
            result[i][j] = A[j][i]
        end
    end 
    return result
end
----------------------------------------------------------------------------------

----------------------------------------------------------------------------------
--/**
-- * @brief Returns the multiplication between two matrices.
-- * @param Matrix A. Example A = {{1,2,3},{4,5,6},{7,8,9}} 
-- * @param Matrix B. Example B = {{1,2,3},{4,5,6},{7,8,9}} 
-- * @returns the multiplication between A and B. Example = mat_mult(A,B)
function mat_mult( A, B )    
    if #A[1] ~= #B then       -- inner matrix-dimensions must agree        
        return nil      
    end  
    local result = {} 
    for i = 1, #A do
        result[i] = {}
        for j = 1, #B[1] do
            result[i][j] = 0
            for k = 1, #B do
                result[i][j] = result[i][j] + A[i][k] * B[k][j]
            end
        end
    end 
    return result
end
----------------------------------------------------------------------------------

Current limitations: The call_script_function method does not take into account the input buffer and does not return the output buffer.

Example in C++: (Please find attached the CoppeliaSim scene)

#include <iostream>
#include <string>
#include <thread>
#include <dqrobotics/DQ.h>
#include <dqrobotics/interfaces/vrep/DQ_VrepInterface.h>
#include <dqrobotics/utils/DQ_LinearAlgebra.h>
#include <dqrobotics/utils/DQ_Constants.h>
#include <Eigen/Dense>

using namespace Eigen;

int main(void)
{
    DQ_VrepInterface vi;
    call_script_data data;

    vi.connect(19997,100,10);
    std::cout << "Starting V-REP simulation..." << std::endl;
    vi.start_simulation();
    std::this_thread::sleep_for(std::chrono::milliseconds(100));

    int interations = 100;
    for (int i=0;i<interations;i++)
    {

        const std::vector<int> input_ints = {1,2,3,4,5,6,7};
        const std::vector<float> input_floats = {3.14, 8.56, 10.22};
        const std::vector<std::string> input_strings = {"DQ_Robotics"};

        // test the call_script_function
        data =  vi.call_script_function("DQRoboticsApiCommandServer", DQ_VrepInterface::ST_CHILD, "test_inputs",
                                       input_ints, input_floats, input_strings, DQ_VrepInterface::OP_BLOCKING);
        std::cout<<"itertation:  "<<i<<std::endl;
        std::cout<<"ret:       \n"<<data.return_code<<std::endl;
        std::cout<<"ret ints:  \n"<<data.output_ints<<std::endl;
        std::cout<<"ret floats: \n "<<data.output_floats<<std::endl;

        for (int j=0; j<data.output_strings.size();j++)
        {
            std::cout<<"ret strings "<<data.output_strings[j]<<std::endl;
        }

        // Test the overloaded call_script_function
        data =  vi.call_script_function("DQRoboticsApiCommandServer", "test_inputs",
                                        input_ints, input_floats, input_strings);

        std::cout<<"itertation:  "<<i<<std::endl;
        std::cout<<"ret:       \n"<<data.return_code<<std::endl;
        std::cout<<"ret ints:  \n"<<data.output_ints<<std::endl;
        std::cout<<"ret floats: \n "<<data.output_floats<<std::endl;

        for (int j=0; j<data.output_strings.size();j++)
        {
            std::cout<<"ret strings "<<data.output_strings[j]<<std::endl;
        }

        // Test the methods extract_mass, extract_inertia_matrix, extract_center_of_mass
        std::string link = "Franka_link2_resp";
        double mass = vi.extract_mass("DQRoboticsApiCommandServer","get_mass",link);
        std::cout<<"mass new: "<<mass<<std::endl;

        MatrixXd inertia_matrix = vi.extract_inertia_matrix("DQRoboticsApiCommandServer","get_inertia",
                                                            link, "absolute_frame");
        //We divided the inertia_matrix by the mass to compare with the VREP dynamic properties directly.
        std::cout<<"Inertia_matrix expressed in absolute frame: \n"<<inertia_matrix/mass<<std::endl;

        inertia_matrix = vi.extract_inertia_matrix("DQRoboticsApiCommandServer","get_inertia",
                                                            link);
        //We divided the inertia_matrix by the mass to compare with the VREP dynamic properties directly.
        std::cout<<"Inertia_matrix expressed in shape frame: \n"<<inertia_matrix/mass<<std::endl;

        VectorXd center_of_mass = vi.extract_center_of_mass("DQRoboticsApiCommandServer","get_center_of_mass",
                                                           link, "absolute_frame");
        std::cout<<"Center of mass expressed in absolute frame: \n"<<center_of_mass<<std::endl;

        center_of_mass = vi.extract_center_of_mass("DQRoboticsApiCommandServer","get_center_of_mass",link);
        std::cout<<"Center of mass expressed in shape frame: \n"<<center_of_mass<<std::endl;

        //std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

    std::cout << "Stopping V-REP simulation..." << std::endl;
    vi.stop_simulation();

    vi.disconnect();

    return 0;
}

What do you think? 😃

Best regards!

Juancho

juanjqo commented 2 years ago

extract_inertial_parameters.zip

juanjqo commented 2 years ago

Hi @mmmarinho. I implemented your suggestions. The new example is given as follows,

#include <iostream>
#include <string>
#include <thread>

#include <dqrobotics/DQ.h>

#include <dqrobotics/interfaces/vrep/DQ_VrepInterface.h>
#include <dqrobotics/utils/DQ_LinearAlgebra.h>
#include <dqrobotics/utils/DQ_Constants.h>
#include <Eigen/Dense>

using namespace Eigen;

int main(void)
{
    DQ_VrepInterface vi;
    call_script_data data;

    vi.connect(19997,100,10);
    std::cout << "Starting V-REP simulation..." << std::endl;
    vi.start_simulation();
    std::this_thread::sleep_for(std::chrono::milliseconds(100));

    int interations = 100;
    for (int i=0;i<interations;i++)
    {

        const std::vector<int> input_ints = {1,2,3,4,5,6,7};
        const std::vector<float> input_floats = {3.14, 8.56, 10.22};
        const std::vector<std::string> input_strings = {"DQ_Robotics", "HI"};

        // test the call_script_function
        data =  vi.call_script_function("DQRoboticsApiCommandServer", DQ_VrepInterface::ST_CHILD, "test_inputs",
                                       input_ints, input_floats, input_strings, DQ_VrepInterface::OP_BLOCKING);
        std::cout<<"itertation:  "<<i<<std::endl;
        std::cout<<"ret:       \n"<<data.return_code<<std::endl;
        std::cout<<"ret ints:  \n"<<data.output_ints<<std::endl;
        std::cout<<"ret floats: \n "<<data.output_floats<<std::endl;

        for (int j=0; j<data.output_strings.size();j++)
        {
            std::cout<<"ret strings "<<data.output_strings[j]<<std::endl;
        }

        // Test the overloaded call_script_function
        data =  vi.call_script_function("DQRoboticsApiCommandServer", "test_inputs",
                                        input_ints, input_floats, input_strings);

        std::cout<<"itertation:  "<<i<<std::endl;
        std::cout<<"ret:       \n"<<data.return_code<<std::endl;
        std::cout<<"ret ints:  \n"<<data.output_ints<<std::endl;
        std::cout<<"ret floats: \n "<<data.output_floats<<std::endl;

        for (int j=0; j<data.output_strings.size();j++)
        {
            std::cout<<"ret strings "<<data.output_strings[j]<<std::endl;
        }

        // Test the methods extract_mass, get_inertia_matrix, get_center_of_mass

        std::string link = "Franka_link5_resp";

        double mass = vi.get_mass(link);
        std::cout<<"mass: "<<mass<<std::endl;
        std::cout<<"mass: "<<vi.get_mass(link, "get_mass")<<std::endl;
        std::cout<<"mass: "<<vi.get_mass(link, "get_mass","DQRoboticsApiCommandServer")<<std::endl;

        MatrixXd inertia_matrix = vi.get_inertia_matrix(link);
        std::cout<<"Inertia_matrix expressed in shape frame:    \n"<<inertia_matrix<<std::endl;
        std::cout<<"Inertia_matrix expressed in absolute frame: \n"<<vi.get_inertia_matrix(link, "absolute_frame")<<std::endl;
        std::cout<<"Inertia_matrix expressed in absolute frame: \n"<<vi.get_inertia_matrix(link, "absolute_frame","get_inertia")<<std::endl;
        std::cout<<"Inertia_matrix expressed in absolute frame: \n"<<vi.get_inertia_matrix(link, "absolute_frame","get_inertia","DQRoboticsApiCommandServer")<<std::endl;

        //We divided the inertia_matrix by the mass to compare with the CoppeliaSim dynamic properties directly.
        std::cout<<"Inertia_matrix expressed in absolute frame and divided by the mass: \n"<<vi.get_inertia_matrix(link, "absolute_frame")/mass<<std::endl;

        VectorXd center_of_mass = vi.get_center_of_mass(link);
        std::cout<<"Center of mass expressed in shape frame: \n"<<vi.get_center_of_mass(link)<<std::endl;
        std::cout<<"Center of mass expressed in absolute frame"<<vi.get_center_of_mass(link, "absolute_frame")<<std::endl;
        std::cout<<"Center of mass expressed in absolute frame"<<vi.get_center_of_mass(link, "absolute_frame", "get_center_of_mass")<<std::endl;
        std::cout<<"Center of mass expressed in absolute frame"<<vi.get_center_of_mass(link, "absolute_frame", "get_center_of_mass","DQRoboticsApiCommandServer")<<std::endl;

        //std::this_thread::sleep_for(std::chrono::milliseconds(10));

    }

    std::cout << "Stopping V-REP simulation..." << std::endl;
    vi.stop_simulation();

    vi.disconnect();

    return 0;
}
juanjqo commented 2 years ago

Update: @dqrobotics/developers I realized that the newest version of CoppeliaSim (4.3.0) allows Python (in addition to Lua) as a programming language for CoppeliaSim scripts. However, according to Coppelia, it would be faster to remotely call a Lua function, instead of a Python function.

https://www.coppeliarobotics.com/helpFiles/en/scripts.htm

In addition, CoppeliaSim 4.3.0 enables the use of the ZeroMQ remote API (Available currently for C++ and Python). According to Coppelia, the ZeroMQ remote API is more efficient, lightweight, and have all functions. remoteApiFlavors

The Legacy Remote API (Currently used by DQ_VrepInterface) only has a subset of functions. (Because of that, in some cases is necessary to use the simxCallScriptFunction. For instance, if we require the mass of an object, we use simxCallScriptFunction to call a Lua function that uses getShapeMass internally on CoppeliaSim).

Using the ZeroMQ remote API we can use getShapeMass directly. Example:

from zmqRemoteApi import RemoteAPIClient

client = RemoteAPIClient()
sim = client.getObject('sim')
linkHandle = sim.getObject('/link2_resp')
client.setStepping(True)
sim.startSimulation()

startTime = sim.getSimulationTime()
while sim.getSimulationTime() - startTime < 5:

    mass = sim.getShapeMass(linkHandle)
    print(mass)
    client.step()
sim.stopSimulation()

This is available in C++ as well. However, in C++, the ZeroMQ remote API requires Boost to compile.

sudo apt-get install libboost-all-dev

Example:

#include "RemoteAPIClient.h"
#include <iostream>
#include <iomanip>

int main()
{
    RemoteAPIClient client;
    client.setStepping(true);
    client.call("sim.startSimulation", nullptr);
    auto linkHandle = client.call("sim.getObject", {"/link2_resp"})[0];

    float simTime=0.0f;
    while (simTime<3.0f)
    {

        auto mass = client.call("sim.getShapeMass",{linkHandle})[0];
        std::cout<<"mass: "<<mass<<std::endl;
        std::cout << "Simulation time: " << std::setprecision(3) << simTime << " [s]" << std::endl;
        client.step();
        simTime=client.call("sim.getSimulationTime")[0].as<float>();
    }    
    client.call("sim.stopSimulation", nullptr);

    return 0;
}

extract_inertial_parameters_v430.zip

(Another topic) In CoppeliaSim 4.3.2, all joints have the same name. 2022-02-01_16-53

mmmarinho commented 2 years ago

@juanjqo Thanks for the info!

@dqrobotics/developers
A small preamble before I state my opinion on this. TLDR: let's stick to the "legacy" remoteAPI for now.

Preamble

Some CoppeliaSim versions ago, they were recommending the use of the B0 framework instead of the "legacy" remoteApi we currently use. In the age of VREP, I also went through their ROS interface API.

BlueZero Interface

B0 is now deprecated.

BlueZero is deprecated. We highly recommend using the ZeroMQ remote API instead.

ROS1 Interface

The ROS interfaces were difficult to keep track of, and because ROS itself changed so much between versions (I'm looking at you, Groovy), their interface was always lagging behind.

This tutorial will try to explain in a simple way how you can manage to have CoppeliaSim ROS enabled, based on ROS Melodic and Catkin build.

ROS2 Interface

Regarding build systems, ROS1 used rosbuild then moved to catkin. Now ROS2 uses another build system entirely, called colcon. ROS2's latest version is supposed to be Galactic.

This tutorial will try to explain in a simple way how you can manage to have CoppeliaSim ROS 2 enabled, based on ROS 2 Foxy.

Lessons learned

As someone who has gone through redesigning my entire software stack several times to make use of the newest versions of Ubuntu, ROS1, and Qt, there are a couple of main lessons to be learned:

  1. Have the least amount of dependencies possible. The best case is if we can depend only on the standard library (in C++ terminology, namespace std, in ROS1 terminology roscpp, rospy) and possible external dependencies are self-contained (see the "legacy" remoteAPI).
  2. Use the LTS version whenever such exists, and, if there is no such version, stick to the API that has been around the longest. To put this into perspective for the current discussion, it would be much easier to deprecate the BlueZero interface than to do that to the "legacy" remoteAPI. The software corpus developed using the "legacy" remoteAPI is too big to ignore.
  3. Design the external API around end-user usage and not the simulator's inner workings. Ideally, we should have a superclass DQ_SimulatorInterface or something of the sort, so that we don't even depend on CoppeliaSim that much. Many of my change requests regarding this PR were related to this.

For CPP only: If something needs Boost to work, be sure you have no other options before making that compromise. Here's a quote from pybind11.

The main issue with Boost.Python—and the reason for creating such a similar project—is Boost. Boost is an enormously large and complex suite of utility libraries that works with almost every C++ compiler in existence. This compatibility has its cost: arcane template tricks and workarounds are necessary to support the oldest and buggiest of compiler specimens. Now that C++11-compatible compilers are widely available, this heavy machinery has become an excessively large and unnecessary dependency.

Don't tell anyone I agree with them or I might be blocked from accessing StackOverflow.

Concrete near future suggestions

PS: The names of the objects in CoppeliaSim 4.3.0 have not changed, just the way the information that is displayed to the user has changed.

Note: until CoppeliaSim V4.2.0, object access was based on object names (with sim.getObjectHandle), which are now deprecated (but still functional for backward compatibility). We however highly recommend to use object paths as described below, to retrieve object handles. Object paths can easily be identified since they start with one of following characters: /, . or : Deprecated object names are not displayed anymore in the scene hierarchy (where object aliases are displayed instead). You can however still see the deprecated name of an object by selecting it: it will be displayed at the top of the rendering page.

juanjqo commented 2 years ago

@mmmarinho, Thank you for your insights and recommendations! 😃

juanjqo commented 2 years ago

Updates: (Feb/09/2022) @mmmarinho, I did all the requested modifications and updated the examples.

The custom structure _call_scriptdata is only on DQ_VrepInterface.cpp (It was removed from DQ_VrepInterface.h).

New public methods:

   //--New ones------
    void     set_joint_target_velocity(const int& handle, const double& angle_dot_rad, const OP_MODES& opmode) const;
    void     set_joint_target_velocity(const std::string& jointname, const double& angle_dot_rad, const OP_MODES& opmode=OP_ONESHOT);
    void     set_joint_target_velocities(const std::vector<int>& handles, const VectorXd& angles_dot_rad, const OP_MODES& opmode) const;
    void     set_joint_target_velocities(const std::vector<std::string>& jointnames, const VectorXd& angles_dot_rad, const OP_MODES& opmode=OP_ONESHOT);

    double   get_joint_velocity(const int& handle, const OP_MODES& opmode) const;
    double   get_joint_velocity(const std::string& jointname, const OP_MODES& opmode=OP_AUTOMATIC);
    VectorXd get_joint_velocities(const std::vector<int>& handles, const OP_MODES& opmode) const;
    VectorXd get_joint_velocities(const std::vector<std::string>& jointnames, const OP_MODES& opmode=OP_AUTOMATIC);

    void     set_joint_torque(const int& handle, const double& torque, const OP_MODES& opmode) const;
    void     set_joint_torque(const std::string& jointname, const double& torque, const OP_MODES& opmode=OP_ONESHOT);
    void     set_joint_torques(const std::vector<int>& handles, const VectorXd& torques, const OP_MODES& opmode) const;
    void     set_joint_torques(const std::vector<std::string>& jointnames, const VectorXd& torques, const OP_MODES& opmode=OP_ONESHOT);

    double   get_joint_torque(const int& handle, const OP_MODES& opmode) const;
    double   get_joint_torque(const std::string& jointname, const OP_MODES& opmode=OP_AUTOMATIC);
    VectorXd get_joint_torques(const std::vector<int>& handles, const OP_MODES& opmode) const;
    VectorXd get_joint_torques(const std::vector<std::string>& jointnames, const OP_MODES& opmode=OP_AUTOMATIC);

    MatrixXd get_inertia_matrix(const std::string& link_name, const REFERENCE_FRAMES& reference_frame=BODY_FRAME, const std::string& function_name = "get_inertia", const std::string& obj_name= "DQRoboticsApiCommandServer");
    MatrixXd get_inertia_matrix(const int& handle, const REFERENCE_FRAMES& reference_frame=BODY_FRAME, const std::string& function_name = "get_inertia", const std::string& obj_name= "DQRoboticsApiCommandServer");

    VectorXd get_center_of_mass(const std::string& link_name, const REFERENCE_FRAMES& reference_frame=BODY_FRAME, const std::string& function_name = "get_center_of_mass", const std::string& obj_name= "DQRoboticsApiCommandServer");
    VectorXd get_center_of_mass(const int& handle, const REFERENCE_FRAMES& reference_frame=BODY_FRAME, const std::string& function_name = "get_center_of_mass", const std::string& obj_name= "DQRoboticsApiCommandServer");

    double get_mass(const std::string& link_name, const std::string& function_name = "get_mass", const std::string& obj_name= "DQRoboticsApiCommandServer");
    double get_mass(const int& handle, const std::string& function_name = "get_mass", const std::string& obj_name= "DQRoboticsApiCommandServer");

New privated methods:

    int _call_script_function(const std::string&  function_name, const std::string&  obj_name, const std::vector<int>& input_ints, const std::vector<float>& input_floats, const std::vector<std::string> &input_strings,
                                int* outIntCnt, int** output_ints, int* outFloatCnt, float** output_floats, int* outStringCnt, char** output_strings,
                                const SCRIPT_TYPES& scripttype = ST_CHILD, const OP_MODES& opmode = OP_BLOCKING);

Updated examples:

Example 1 (_call_script_function):


#include <iostream>
#include <string>
#include <thread>

#include <dqrobotics/DQ.h>

#include <dqrobotics/interfaces/vrep/DQ_VrepInterface.h>
#include <dqrobotics/utils/DQ_LinearAlgebra.h>
#include <dqrobotics/utils/DQ_Constants.h>
#include <Eigen/Dense>

using namespace Eigen;

int main(void)
{
    DQ_VrepInterface vi;

    vi.connect(19997,100,10);
    std::cout << "Starting V-REP simulation..." << std::endl;
    vi.start_simulation();
    std::this_thread::sleep_for(std::chrono::milliseconds(100));

    int iterations = 100;
    for (int i=0;i<iterations;i++)
    {
        // Test the methods extract_mass, get_inertia_matrix, get_center_of_mass

        std::string link = "Franka_link5_resp";
        int handle = vi.get_object_handle(link);

#include <iostream>
#include <string>
#include <thread>

#include <dqrobotics/DQ.h>

#include <dqrobotics/interfaces/vrep/DQ_VrepInterface.h>
#include <dqrobotics/utils/DQ_LinearAlgebra.h>
#include <dqrobotics/utils/DQ_Constants.h>
#include <Eigen/Dense>

using namespace Eigen;

int main(void)
{
    DQ_VrepInterface vi;

    vi.connect(19997,100,10);
    std::cout << "Starting V-REP simulation..." << std::endl;
    vi.start_simulation();
    std::this_thread::sleep_for(std::chrono::milliseconds(100));

    int iterations = 100;
    for (int i=0;i<iterations;i++)
    {
        // Test the methods extract_mass, get_inertia_matrix, get_center_of_mass

        std::string link = "Franka_link5_resp";
        int handle = vi.get_object_handle(link);

               double mass = vi.get_mass(link);
        std::cout<<"1. mass: "<<mass<<std::endl;
        std::cout<<"2. mass: "<<vi.get_mass(link, "get_mass")<<std::endl;
        std::cout<<"3. mass: "<<vi.get_mass(link, "get_mass","DQRoboticsApiCommandServer")<<std::endl;

        std::cout<<"4. mass: "<<vi.get_mass(handle)<<std::endl;
        std::cout<<"5. mass: "<<vi.get_mass(handle, "get_mass")<<std::endl;
        std::cout<<"6. mass: "<<vi.get_mass(handle, "get_mass","DQRoboticsApiCommandServer")<<std::endl;

        //std::cout<<"mass: "<<vi.get_mass_exp(link)<<std::endl;

        MatrixXd inertia_matrix = vi.get_inertia_matrix(link);
        std::cout<<"1. Inertia_matrix expressed in shape frame:    \n"<<inertia_matrix<<std::endl;
        std::cout<<"2. Inertia_matrix expressed in absolute frame: \n"<<vi.get_inertia_matrix(link, DQ_VrepInterface::ABSOLUTE_FRAME)<<std::endl;
        std::cout<<"3. Inertia_matrix expressed in absolute frame: \n"<<vi.get_inertia_matrix(link, DQ_VrepInterface::ABSOLUTE_FRAME,"get_inertia")<<std::endl;
        std::cout<<"4. Inertia_matrix expressed in absolute frame: \n"<<vi.get_inertia_matrix(link, DQ_VrepInterface::ABSOLUTE_FRAME,"get_inertia","DQRoboticsApiCommandServer")<<std::endl;

        std::cout<<"5. Inertia_matrix expressed in shape frame:    \n"<<vi.get_inertia_matrix(handle)<<std::endl;
        std::cout<<"6. Inertia_matrix expressed in absolute frame: \n"<<vi.get_inertia_matrix(handle, DQ_VrepInterface::ABSOLUTE_FRAME)<<std::endl;
        std::cout<<"7. Inertia_matrix expressed in absolute frame: \n"<<vi.get_inertia_matrix(handle, DQ_VrepInterface::ABSOLUTE_FRAME,"get_inertia")<<std::endl;
        std::cout<<"8. Inertia_matrix expressed in absolute frame: \n"<<vi.get_inertia_matrix(handle, DQ_VrepInterface::ABSOLUTE_FRAME,"get_inertia","DQRoboticsApiCommandServer")<<std::endl;

        //We divided the inertia_matrix by the mass to compare with the CoppeliaSim dynamic properties directly.
        std::cout<<"9. Inertia_matrix expressed in absolute frame and divided by the mass: \n"<<vi.get_inertia_matrix(link, DQ_VrepInterface::ABSOLUTE_FRAME)/mass<<std::endl;

        VectorXd center_of_mass = vi.get_center_of_mass(link);
        std::cout<<"1. Center of mass expressed in shape frame: \n"<<vi.get_center_of_mass(link)<<std::endl;
        std::cout<<"2. Center of mass expressed in absolute frame: \n"<<vi.get_center_of_mass(link, DQ_VrepInterface::ABSOLUTE_FRAME)<<std::endl;
        std::cout<<"3. Center of mass expressed in absolute frame: \n"<<vi.get_center_of_mass(link, DQ_VrepInterface::ABSOLUTE_FRAME, "get_center_of_mass")<<std::endl;
        std::cout<<"4. Center of mass expressed in absolute frame: \n"<<vi.get_center_of_mass(link, DQ_VrepInterface::ABSOLUTE_FRAME, "get_center_of_mass","DQRoboticsApiCommandServer")<<std::endl;

        std::cout<<"5. Center of mass expressed in shape frame: \n"<<vi.get_center_of_mass(handle)<<std::endl;
        std::cout<<"6. Center of mass expressed in absolute frame: \n"<<vi.get_center_of_mass(handle, DQ_VrepInterface::ABSOLUTE_FRAME)<<std::endl;
        std::cout<<"7. Center of mass expressed in absolute frame: \n"<<vi.get_center_of_mass(handle, DQ_VrepInterface::ABSOLUTE_FRAME, "get_center_of_mass")<<std::endl;
        std::cout<<"8. Center of mass expressed in absolute frame: \n"<<vi.get_center_of_mass(handle, DQ_VrepInterface::ABSOLUTE_FRAME, "get_center_of_mass","DQRoboticsApiCommandServer")<<std::endl;

        //std::this_thread::sleep_for(std::chrono::milliseconds(10));

    }

    std::cout << "Stopping V-REP simulation..." << std::endl;
    vi.stop_simulation();

    vi.disconnect();

    return 0;
}

        //std::this_thread::sleep_for(std::chrono::milliseconds(10));

    }

    std::cout << "Stopping V-REP simulation..." << std::endl;
    vi.stop_simulation();

    vi.disconnect();

    return 0;
}

Example 2 (joint velocities):

#include <iostream>
#include <string>
#include <thread>

#include <dqrobotics/DQ.h>

#include <dqrobotics/interfaces/vrep/DQ_VrepInterface.h>
#include <dqrobotics/utils/DQ_LinearAlgebra.h>
#include <dqrobotics/utils/DQ_Constants.h>
#include <Eigen/Dense>
#include <dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h>
#include <dqrobotics/robot_control/DQ_PseudoinverseController.h>
#include <dqrobotics/robot_control/DQ_KinematicController.h>

using namespace Eigen;

int main(void)
{
    DQ_VrepInterface vi;
    vi.connect(19997,100,10);
    vi.set_synchronous(true);
    std::cout << "Starting V-REP simulation..." << std::endl;
    vi.start_simulation();
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    int iterations = 1000;

    // robot definition
    Matrix<double, 5, 7> robot_dh;
    robot_dh << 0, 0, 0, 0, 0, 0, 0,                           // theta
                0.333, 0, 3.16e-1, 0, 3.84e-1, 0, 1.07e-1,     // d
                0, 0, 8.25e-2, -8.25e-2, 0, 8.8e-2, 0,         // a
                -M_PI_2, M_PI_2, M_PI_2, -M_PI_2, M_PI_2, M_PI_2, 0, // alpha
                 0,0,0,0,0,0,0;
    DQ_SerialManipulatorDH franka(robot_dh);
    DQ robot_base = 1 + E_ * 0.5 * DQ(0, 0.0413, 0, 0);
    franka.set_base_frame(robot_base);
    franka.set_reference_frame(robot_base);

    std::vector<std::string> jointnames = {"Franka_joint1", "Franka_joint2",
                                            "Franka_joint3", "Franka_joint4",
                                            "Franka_joint5", "Franka_joint6",
                                            "Franka_joint7"};

    DQ_PseudoinverseController controller(&franka);
    controller.set_gain(0.5);
    controller.set_damping(0.05);
    controller.set_control_objective(DQ_robotics::Translation);
    controller.set_stability_threshold(0.00001);

    DQ xdesired = 1 + E_*0.5*DQ(0, 0.2, 0.3, 0.3);

    //for (int i=0;i<iterations;i++)
    int i=0;
    while (not controller.system_reached_stable_region())
    {

        VectorXd q = vi.get_joint_positions(jointnames);
        VectorXd u = controller.compute_setpoint_control_signal(q, vec4(xdesired.translation()));
        std::cout << "error: " <<norm(controller.get_last_error_signal())<<" Iteration: "<<i<<std::endl;
        std::cout<< "Is stable?: "<<controller.system_reached_stable_region()<<std::endl;

        vi.set_joint_target_velocities(jointnames, u);
        vi.trigger_next_simulation_step();
        std::cout<< "u:     "<<u<<std::endl;
        std::cout<< "q_dot: "<<vi.get_joint_velocities(jointnames)<<std::endl;
        i++;

    }
    std::cout << "Stopping V-REP simulation..." << std::endl;
    vi.stop_simulation();
    vi.disconnect();
    return 0;
}

CoppeliaSim Scene (Compatible with both examples) joint_velocities_test.zip

Example 3 (Torques):

This example works with the CoppeliaSim "joint_torques_no_gravity.ttt"

#include <iostream>
#include <string>
#include <thread>
#include <fstream>

#include <dqrobotics/DQ.h>

#include <dqrobotics/interfaces/vrep/DQ_VrepInterface.h>
//#include <dqrobotics/utils/DQ_LinearAlgebra.h>
//#include <dqrobotics/utils/DQ_Constants.h>
#include <Eigen/Dense>

using namespace Eigen;

int main(void)
{
    DQ_VrepInterface vi;
    vi.connect(19997,100,10);
    vi.set_synchronous(true);
    std::cout << "Starting V-REP simulation..." << std::endl;
    vi.start_simulation();
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    int iterations = 1000;

    std::vector<std::string> jointnames = {"Franka_joint1", "Franka_joint2",
                                            "Franka_joint3", "Franka_joint4",
                                            "Franka_joint5", "Franka_joint6",
                                            "Franka_joint7"};

    VectorXd vec_torques(7);

    double Kp = 0.04; //4.5
    double Kv = 3*sqrt(Kp);
    VectorXd qd = VectorXd(7);
    qd <<-0.70, -0.10, 1.66, -2.34,0.40, 1.26, 0.070;

    VectorXd g_qd = VectorXd(7);
    Matrix<double, 7,3> list_torques;

    std::ofstream list_torques_ref;
    list_torques_ref.open("list_torques_ref.csv");

    std::ofstream list_torques_read;
    list_torques_read.open("list_torques_read.csv");

    for (int i=0;i<iterations;i++)
    {
        VectorXd q = vi.get_joint_positions(jointnames);
        VectorXd qerror = qd-q;
        VectorXd q_dot = vi.get_joint_velocities(jointnames);
        VectorXd qerror_dot = -q_dot;

        vec_torques = Kp*qerror + Kv*qerror_dot ;
        vi.set_joint_torques(jointnames, vec_torques);
        vi.trigger_next_simulation_step();

        VectorXd vec_torques_read = vi.get_joint_torques(jointnames);

        list_torques.col(0) = vec_torques;
        list_torques.col(1) = vec_torques_read;
        list_torques.col(2) = list_torques.col(0)-list_torques.col(1);

        list_torques_ref <<vec_torques(0)<<","<<vec_torques(1)<<","<<vec_torques(2)<<
                           ","<<vec_torques(3)<<","<<vec_torques(4)<<","<<vec_torques(5)<<
                           ","<<vec_torques(6)<<","<<'\n';

        list_torques_read <<vec_torques_read(0)<<","<<vec_torques_read(1)<<","<<vec_torques_read(2)<<
                           ","<<vec_torques_read(3)<<","<<vec_torques_read(4)<<","<<vec_torques_read(5)<<
                           ","<<vec_torques_read(6)<<","<<'\n';

        std::cout<< "torques ref:   torques read:    error:"<<std::endl;
        std::cout<<list_torques<<std::endl;

        std::cout<< "Applying torques..."<<iterations-i<<std::endl;
        std::cout<< "Error..."<<qerror.norm()<<std::endl;
        //std::this_thread::sleep_for(std::chrono::milliseconds(40));
    }
    list_torques_ref.close();
    list_torques_read.close();

    std::cout << "Stopping V-REP simulation..." << std::endl;
    vi.stop_simulation();
    vi.disconnect();
    return 0;
}

joint_torques_no_gravity.zip

Results:

The following Matlab code shows the joint torques (references and measurements) to illustrate the results of the methods set_joint_torques() and get_joint_torques(); 

clear all
close all
clc

torques_ref = readmatrix('list_torques_ref.csv')';
torques_read = readmatrix('list_torques_read.csv')';

h1 = figure;
set(h1, 'DefaultTextFontSize', 10);
set(h1, 'DefaultAxesFontSize', 10); % [pt]
set(h1, 'DefaultAxesFontName', 'mwa_cmr10');
set(h1, 'DefaultTextFontName', 'mwa_cmr10');
set(h1, 'Units', 'centimeters');
pos = get(h1, 'Position');
pos(3) = 2*20; % Select the width of the figure in [cm] 17
pos(4) = 2*10; % Select the height of the figure in [cm] 6
set(h1, 'Position', pos);
set(h1, 'PaperType', 'a4letter');
set(h1,'PaperPositionMode','auto')
set(h1, 'Renderer', 'Painters');

a = 2;
b = 4;
w = 4;
fontsize = 20;
for i=1:7
subplot(a,b,i);
plot(torques_ref(i,:),'b', 'LineWidth',w);
hold on
plot(torques_read(i,:),':r', 'LineWidth',3);
set(gca, 'FontSize', fontsize );
fig = gcf;
fig.Color = [1 1 1];
box('off');

%title('$\tau$', 'Interpreter','latex', 'Rotation', 0)
title(['\tau_',num2str(i)])
end
legend('Reference', 'measurement')

results

juanjqo commented 2 years ago

@juanjqo As discussed today, we just need the matching examples to be added to the correct repo.

@mmmarinho I added the examples here

mmmarinho commented 2 years ago

@juanjqo Merge with the current master one more time and the build should succeed.