[LANGUAGE INCOMPATIBILITY] The DQ_KinematicController does not reset the stable region flag.

[marcos-pereira]

Describe the missing/unexpected functionality
The DQ_KinematicController.m reset_stability_counter() does not reset the system_reached_stable_region_ as in the C++ version DQ_KinematicController.cpp.

Personally, I have used this functionality in C++ and missed it in Matlab. However, I am not sure if there should be a separate method to reset the system_reached_stable_region or not.

As a temporary solution, I just added the functionality myself.

If needed, I could do a pull request to add this functionality two weeks from now.

Environment:

• dqrobotics version: the version from the Master branch.

[bvadorno]

Hi @marcos-pereira,

If you want to continue this issue, please adapt it to our current standards. The information you gave me needs to be more comprehensive. More specifically:

Describe the missing/unexpected functionality

• A clear and concise description of what is happening.
• Add error messages to the output (if any)
• MAKE A MINIMAL EXAMPLE TO SHOW WHAT YOU WANT TO SHOW; THE LONGER YOUR CODE IS, THE LESS LIKELY YOU ARE TO GET PROPER SUPPORT

Matlab behavior (if applicable)

MATLAB SCRIPT

%% Your MATLAB script here

MATLAB OUTPUT

[Paste your output here]

Many thanks,
Bruno

[marcos-pereira]

Hi @bvadorno ,

Here follows a more detailed description of the issue:

A clear and concise description of what is happening
The DQ_KinematicController reset_stability_counter() method does not reset the system_reached_stable_region_ attribute. This way, if we wish to run a trajectory of references with the controller, it will run only the first reference because on the second reference the controller will be already stable even if we call the reset_stability_counter() method.

Minimal example in MATLAB
For the sake of the example, I borrowed some values from this constrained control example.

include_namespace_dq

kuka = KukaLwr4Robot.kinematics();

q = [pi/2, -1.8*pi/2, 0, -1.5*pi/2, -1.5*pi/2, 0, 0]';

T = 0.001;

solver = DQ_QuadprogSolver;

controller = DQ_ClassicQPController(kuka,solver);
controller.set_gain(100);
controller.set_stability_threshold(0.0001);
controller.set_control_objective(ControlObjective.Pose);

figure;

x_pose1 = 1 + E_*(1/2)*(-0.15*i_ -0.5*j_ + 0.3*k_);
x_pose2 = 1 + E_*(1/2)*(-0.15*i_ +0.5*j_ + 0.3*k_);

pose_trajectory = {};
pose_trajectory{1} = x_pose1;
pose_trajectory{2} = x_pose2;
num_poses = length(pose_trajectory);

plot(x_pose1, 'scale', 0.2, 'name', 'desired pose');
hold on
plot(x_pose2, 'scale', 0.2, 'name', 'desired pose');
hold on
plot(kuka,q', 'nojoints');

pose_count = 1;

while pose_count <= num_poses
    x_reference = pose_trajectory{pose_count};

    while ~controller.system_reached_stable_region()                
        u = controller.compute_setpoint_control_signal(q, vec8(x_reference));
    
        q = q + T*u;
        plot(kuka,q', 'nojoints');
    
        drawnow
    
    end
    % The issue occurs here:
    % since the reset_stability_counter does not reset the 
    % system_reached_stable_region_, the controller state remains 
    % stable and the second pose is never reached.
    controller.reset_stability_counter();
    pose_count = pose_count + 1;
end

Expected behavior
The manipulator end-effector should go to pose1 and then to pose2. However, since the system_reached_stable_region_ is not reset, the controller is already stable when running pose2.

Possible solution
We could do the same as in the DQ_KinematicController.cpp: reset the system_reached_stable_region_ inside the method reset_stability_counter(). Alternatively, we could create a method to reset the attribute, but then we also need to fix the C++ implementation.

Let me know if you need more information. Let us discuss the best solution and then I can open a PR.

Best,
Marcos

[ffasilva]

Hi @marcos-pereira,

Thank you for the minimal example. This issue was solved in #111.

@bvadorno, could you please mark this as solved?

Kind regards,
Frederico