[LANGUAGE INCOMPATIBILITY] The DistanceToPlane control objective is missing.

[marcos-pereira]

Hello @mmmarinho, would it be possible to add the DistanceToPlane control objective to the C++ implementation?

Describe the missing/unexpected functionality The DistanceToPlane control objective is missing on the C++ implementation.

Matlab behavior (if applicable) No execution errors occur.

MATLAB SCRIPT

clc
close all
clear all

include_namespace_dq

kuka = KukaLwr4Robot.kinematics();

solver = DQ_QuadprogSolver;

plane = k_;

controller = DQ_ClassicQPController(kuka,solver);
controller.set_gain(10);
controller.set_stability_threshold(0.0001);
controller.set_damping(0.01);
controller.set_control_objective(ControlObjective.DistanceToPlane);

controller.set_max_stability_counter(100);
controller.set_target_primitive(plane);

MATLAB OUTPUT

<No output expected>

C++ behavior (if applicable) The control objective DistanceToPlane is not found. The method set_max_stability_counter is not found. The method set_target_primitive is not found.

C++ Code

#include <iostream>

#include <dqrobotics/robot_modeling/DQ_SerialManipulator.h>
#include <dqrobotics/robots/KukaLw4Robot.h>
#include <dqrobotics/robot_control/DQ_QuadraticProgrammingController.h>
#include <dqrobotics/robot_control/DQ_ClassicQPController.h>
#include <dqrobotics/solvers/DQ_CPLEXSolver.h>

using namespace DQ_robotics;

void test_issue()
{
    const DQ plane = DQ_robotics::k_;

    DQ_SerialManipulator robot = KukaLw4Robot::kinematics();

    // Constrained QP controller
    DQ_CPLEXSolver cplex_solver;
    DQ_ClassicQPController controller(&robot, &cplex_solver);       
    controller.set_control_objective(ControlObjective::DistanceToPlane);
    controller.set_gain(10.0);
    controller.set_damping(0.01);
    controller.set_max_stability_counter(100);
    controller.set_target_primitive(plane);

}

int main(void)
{
    test_issue();
    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.1)
project(issue_DistanceToPlane)

# find_package (Threads REQUIRED)

set (CMAKE_CXX_STANDARD 11)
# add_compile_options(-std=c++14)

set( CPLEX_PATH /opt/ibm/ILOG/CPLEX_Studio129 )

add_definitions(-DIL_STD)

link_directories(
    ${CPLEX_PATH}/cplex/lib/x86-64_linux/static_pic
    ${CPLEX_PATH}/concert/lib/x86-64_linux/static_pic
    ${CPLEX_PATH}/bin/glnxa64
    )

include_directories(
    ${CPLEX_PATH}/concert/include
    ${CPLEX_PATH}/cplex/include
    )

add_executable(${PROJECT_NAME} issue_DistanceToPlane.cpp)

target_link_libraries(${PROJECT_NAME}  
  # pthread
  # Threads::Threads
  -ldqrobotics  
  ilocplex
  cplex
  concert
  m
  pthread
  ${CMAKE_DL_LIBS}
  # dl  
)

C++ OUTPUT The compilation throws the following errors.

issue_DistanceToPlane.cpp:20:56: error: ‘DistanceToPlane’ is not a member of ‘DQ_robotics::ControlObjective’
     controller.set_control_objective(ControlObjective::DistanceToPlane);

...

issue_DistanceToPlane.cpp:23:16: error: ‘class DQ_robotics::DQ_ClassicQPController’ has no member named ‘set_max_stability_counter’
     controller.set_max_stability_counter(100);

...
issue_DistanceToPlane.cpp:24:16: error: ‘class DQ_robotics::DQ_ClassicQPController’ has no member named ‘set_target_primitive’; did you mean ‘attached_primitive_’?
     controller.set_target_primitive(plane);
                ^~~~~~~~~~~~~~~~~~~~
                attached_primitive_

Environment:

• OS: ubuntu 18.04
• C++ dqrobotics version: 19.10.0-0 201910290536 ubuntu18.04.1
• MATLAB version 2018a

Additional context The DistanceToPlane control objective was recently added to the DQ Robotics in Matlab. The DistanceToPlane is defined on the DQ_KinematicController.m, but it is not defined on the DQ_KinematicController.h.

[mmmarinho]

Hello, @marcos-pereira,

If I'm not mistaken you are referring to the changes done in https://github.com/dqrobotics/matlab/commit/dc11509645ecc0516187e00e1bad2699f0113ca8 which are not part of any major release yet.

I'll make the changes on the dev branch eventually, but it might take a while.

Cheers, Murilo

[marcos-pereira]

Hi, @mmmarinho ,

That's correct.

May I consider implementing it myself and doing a pull request? In the case I have some doubt, I could ask here. I would work on that in the following days.

Best regards, Marcos

[mmmarinho]

Hello, @marcos-pereira,

You are welcome to try changing the code yourself and testing it out. However, I won't be able to accept your pull request into the main repository.

The reason for this is that changing the CPP code is only 5% of the work, I also need to change the Python bindings and add integrated testing to cover all new methods in Matlab, in Python, and then square out any bugs in Python and in CPP. Otherwise, every small change in the main repository will cause an influx of issues that I want to avoid.

In the (hopefully near) future, @bvadorno and I will come up with specific rules to those who want to contribute to the main repositories. As of now, your contributions are still very much welcome in the "examples" repositories.

Kind regards, Murilo

[mmmarinho]

Hello, @marcos-pereira,

I used your example to test the new code and it should be working fine. Let me know if something comes up.

Just remember that I'm pushing the changes to the development PPA.

Kind regards, Murilo

[marcos-pereira]

Hi, @mmmarinho ,

Thank you very much for the quick response! I will be testing it in the following weeks.

Cheers,

Marcos