This PR adds force/torque actuation/reading methods to the class 'DQ_VrepInterface'

[ffasilva]

Main instructions

By submitting this pull request, you automatically agree that you have read and accepted the following conditions:

• Anyone wanting to propose a new modification or introduce new functionality should reach out to the team first, as proposed modifications that do not comply with the library's development philosophy and style, do not follow the library's architecture, do not introduce a clear and general benefit to the library other than to the person who proposed the modification will likely be rejected with no further discussion.
• Support for DQ Robotics is given voluntarily and it is not the developers' role to educate and/or convince anyone of their vision.
• Any possible response and its timeliness will be based on the relevance, accuracy, and politeness of a request and the following discussion.
• You are familiar with the development workflow.
• Each pull request should contain only individual changes (several changes of the same type are allowed on the same pull request).
• Refactoring or modifying internal implementation that is working is not allowed unless comprehensively discussed with and approved by @bvadorno and @mmmarinho.

Description of changes

Hi, @dqrobotics/developers,

Added the following methods to the class DQ_VrepInterface:

• get_joint_torques()
• set_joint_torques()
• get_force_sensor_readings()

Also added the class FrankaEmikaPandaVrepRobot.

Kind regards,
Frederico

Rationale

All the new methods added to the class DQ_VrepInterface are related to force/torque actuation/reading. The class FrankaEmikaPandaVrepRobot is used for the V-REP example of application of the new methods given in 8.

[juanjqo]

Hi @ffasilva, thank you for your PR.

[suggestion]
I think it is better/faster right now if we focus first on the tasks previously discussed in pending tasks. For instance, the implementation of methods set_joint_torques(), get_joint_torques(), get_inertia_matrix(), get_center_of_mass(), get_mass(). This is, the method get_force_sensor_readings() could be added later in a different PR. Especially, because the signature you are proposing

function [force_vec, torque_vec, return_code, state_sensor] = get_force_sensor_readings(obj,handle,opmode)
end

does not follow the signature style of other methods in DQ_VrepInterface.m or DQ_VrepInterface.h. Therefore, these types of PRs usually take a long time to be accepted.

Best regards,

Juancho

Update:

Additional suggestions:

• I would recommend first adding the class DQ_SerialVrepRobot, before adding the FrankaEmikaPandaVrepRobot class. This would allow for a discussion about the DQ_SerialVrepRobot.h implementation (if any), and keep the same implementation of FrankaEmikaPandaVrepRobot class in both versions (Matlab and C++/Python).

• I would recommend modifying the signature of the method get_joint_torques() from

 [joint_torques,return_code] = get_joint_torques(obj,handles,opmode)

To

 joint_torques = get_joint_torques(obj,handles,opmode)

to keep the same signature style of the other methods in DQ_VrepInterface.m or DQ_VrepInterface.h.

• Finally, I recommend adjusting the documentation of the proposed methods to match the style of the class.
  Example:
  Proposed:

            %% Get the readings of a force sensor in V-REP. For joints that are in 'Torque/force mode' in V-REP
            %%  >> force_sensor_name = 'Sensor_name_in_VREP';
            %%  >> [force_vec, torque_vec] = vi.get_force_sensor_readings(force_sensor_handle); 

Expected (this is an example of the method get_joint_velocities):

function joint_velocities = get_joint_velocities(obj,jointnames,opmode)
            % This method gets the joint velocities of a robot in the CoppeliaSim scene.
            % Usage:
            %      Recommended:
            %      joint_velocities = get_joint_velocities(jointnames);
            %
            %      Advanced:
            %      joint_velocities = get_joint_velocities(jointnames, opmode)   
            %
            %          jointnames: The joint names.
            %          (optional) opmode: The operation mode. If not specified, 
            %                       the opmode will be set automatically. 
            %                          
            %                        You can use the following modes:
            %                           OP_BLOCKING 
            %                           OP_STREAMING 
            %                           OP_ONESHOT 
            %                           OP_BUFFER;
            %
            %                       Check this link for more details:
            %                       https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm
            %
            % Example:
            %      jointnames={'LBR4p_joint1','LBR4p_joint2','LBR4p_joint3','LBR4p_joint4',...
            %                  'LBR4p_joint5','LBR4p_joint6','LBR4p_joint7'};
            %
            %      % Recommended:
            %      joint_velocities = get_joint_velocities(jointnames);
            %
            %      % Advanced usage:
            %      joint_velocities = get_joint_velocities(jointnames, OP_ONESHOT);

[ffasilva]

Hi @juanjqo,

Thank your for the suggestions. I have made the following modifications in the pull request:

• Removed the method get_force_sensor_readings in a5bb2bf
• Removed the class FrankaEmikaPandaVrepRobot in fb6d048
• Improved the documentation of methods get_joint_torques and set_joint_torques in 1561d25

However, I did not modify the signature of the method get_joint_torques as it currently follows the one from the method

[joint_positions,retval] = get_joint_positions(obj,jointnames,opmode)

in the class DQ_VrepInterface. Furthermore, as it is, get_joint_torques still allows the simpler use

 joint_torques = get_joint_torques(obj,handles,opmode)

Finally, I have added the methods send_tau_to_vrep and get_tau_from_vrep to the class LBR4pVrepRobot in fb6d048. I need to have some DQ_VrepRobot to use in the example given in 8, which showcases the methods get_joint_torques and set_joint_torques.

Kind regards,
Frederico

[juanjqo]

@dqrobotics/developers

Hi @ffasilva. As far as I know, the signature of the method

[joint_positions,retval] = get_joint_positions(obj,jointnames,opmode)

, which was implemented by @mmmarinho in bb3a836, is supposed to be deprecated. The current signatures for public getter methods only return a single parameter.

Example:

C++:

    VectorXd get_joint_positions(const std::vector<std::string>& jointnames, const OP_MODES& opmode=OP_AUTOMATIC);
    VectorXd get_joint_velocities(const std::vector<std::string>& jointnames, const OP_MODES& opmode=OP_AUTOMATIC);
    VectorXd get_joint_torques(const std::vector<std::string>& jointnames, const OP_MODES& opmode=OP_AUTOMATIC);

Bindings (Python):

dqvrepinterface_py.def("get_joint_positions",
                           (VectorXd (DQ_VrepInterface::*) (const std::vector<std::string>&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_joint_positions,
                           "Get joint positions",
                           py::arg("jointnames")=std::vector<std::string>(),
                           py::arg("opmode")=DQ_VrepInterface::OP_AUTOMATIC);
                           
dqvrepinterface_py.def("set_joint_target_velocities",
                           (void (DQ_VrepInterface::*) (const std::vector<std::string>&, const VectorXd&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_joint_target_velocities,
                           "Set joint velcoties",
                           py::arg("jointnames")=std::vector<std::string>(),
                           py::arg("angles_dot_rad")=VectorXd::Zero(1),
                           py::arg("opmode")=DQ_VrepInterface::OP_ONESHOT); 
                           
dqvrepinterface_py.def("get_joint_torques",
                           (VectorXd (DQ_VrepInterface::*) (const std::vector<std::string>&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_joint_torques,
                           "Get joint torques",
                           py::arg("jointnames")=std::vector<std::string>(),
                           py::arg("opmode")=DQ_VrepInterface::OP_AUTOMATIC);                                                   

If we decide to change that, we would need to reimplement all methods in C++ and Python in order to keep the compatibility between the languages. It looks like a lot of work for little gain. Maybe @bvadorno and @mmmarinho could guide us concerning this topic.

Best regards,

Juancho

[mmmarinho]

@dqrobotics/developers

My 2 cents on this.

The retval in

[joint_positions,retval] = get_joint_positions(obj,jointnames,opmode)

should be removed to comply with the other signatures in MATLAB and other languages.

The retval is not supposed to be visible to the user and is something not available in other methods. If there's any need for retval, the class needs to be restructured to support it everywhere. The class as I envisioned it first was primarily to rely on OP_AUTOMATIC and abstract away any other implementation details of CoppeliaSim's (legacy) remote API.

If the MATLAB code is depending on it for some reason (hopefully it isn't), it was a mistake in my first implementation of this method, and it should be fixed to not need it.

[bvadorno]

Hi @dqrobotics/developers,

Following @juanjqo and @mmmarinho's comments, it's settled. Let's remove retval from the signature of get_joint_positions.

@ffasilva, may you adjust your pull request accordingly?

Many thanks,
Bruno

[bvadorno]

Hi @ffasilva,

Any news on this?

All the best,
Bruno

[juanjqo]

Hi @ffasilva,

It looks like some of your commit messages do not follow the DQ Robotics convention described in CONTRIBUTING.md. Please update the message style in your new commits accordingly.

Please indicate in your commit message, using brackets, the modified file. For instance, if you modified the class DQ_SerialManipulator, then you would do the following:

git commit -m "[DQ_SerialManipulator] your_message_explaining_the modification."

Best regards,

Juancho

[ffasilva]

Hi @juanjqo,

I changed the signature of get_joint_torques to

joint_torques = get_joint_torques(obj,handles,opmode)

Kind regards,
Frederico

[juanjqo]

Hi @ffasilva, thanks.

There are no methods send_tau_to_vrep() and get_tau_from_vrep() in the class LBR4VrepRobot in the C++/Python version. Therefore, their addition to the Matlab version would be a step towards the divergence between the versions. Instead, such methods are implemented by inheriting the class DQ_SerialVrepRobot, which uses more descriptive names as set_configuration_space_torques and get_configuration_space_torques, respectively.

Therefore, I recommend removing the modifications in LBR4VrepRobot.m and updating your example accordingly to advance faster in this PR.

Best regards,

Juancho

[bvadorno]

Hi @juanjqo and @ffasilva,

How is this progressing?

Best wishes,
Bruno

[ffasilva]

Hi @juanjqo,

I removed the methods send_tau_to_vrep() and get_tau_from_vrep() from the class LBR4VrepRobot in 937d208.

I didn't update the example because it can't do anything without those methods. I suggest we leave it open, and I'll updated it after I implement the class DQ_SerialVrepRobot.

@bvadorno, I was going through the tasks following the order we had in the checklist. However, it's clear to me know it's better to skip to the implementation of the class DQ_SerialVrepRobot before working on the remaining tasks.

Kind regards,
Frederico

[juanjqo]

@ffasilva thank you for your effort!

Your current PR implements the methods get_joint_torques() and set_joint_torques() but it looks like your proposed example does not use them. Please update your proposed example to test both methods.

Best regards,

Juancho

[ffasilva]

Hi @juanjqo,

@ffasilva thank you for your effort!

Your current PR implements the methods get_joint_torques() and set_joint_torques() but it looks like your proposed example does not use them. Please update your proposed example to test both methods.

Best regards,

Juancho

Updated in a50de7e.

Kind regards,
Frederico

[juanjqo]

@ffasilva thank you for your example. I just added minor comments.

[juanjqo]

@bvadorno I have no more comments for this PR.