[BUG] The compute_setpoint_control_signal() is throwing a runtime error when using the DQ_DifferentialDriveRobot

[marcos-pereira]

Bug description

• The DQ_ClassicQPController compute_setpoint_control_signal() is throwing a runtime error when it is used with the DQ_DifferentialDriveRobot.

To Reproduce

Code

import numpy as np
from dqrobotics import *
from dqrobotics.robot_modeling import DQ_DifferentialDriveRobot
from dqrobotics.solvers import DQ_QuadprogSolver
from dqrobotics.robot_control import DQ_ClassicQPController, ControlObjective

def main():
    mobile_base = DQ_DifferentialDriveRobot(1, 1)

    qp_solver = DQ_QuadprogSolver()
    mobile_base_controller = DQ_ClassicQPController(mobile_base, qp_solver)
    mobile_base_controller.set_control_objective(ControlObjective.Pose)
    control_gain = 10.0
    mobile_base_controller.set_gain(control_gain)
    control_damping = 0.001
    mobile_base_controller.set_damping(control_damping)

    q = np.array([0.12, 0.0, 0.0])
    J = mobile_base.pose_jacobian(q)

    x_reference = 1.0
    y_reference = 1.0
    phi_reference = 0.0
    q_reference = np.array([x_reference, y_reference, phi_reference])
    print("q_reference")
    print(q_reference)
    task_reference = mobile_base.fkm(q_reference)

    u_qdot = mobile_base_controller.compute_setpoint_control_signal(q, vec8(task_reference))


if __name__ == '__main__':

    main()

Output

q_reference
[1. 1. 0.]
Traceback (most recent call last):
  File "issues.py", line 36, in <module>
    main()
  File "issues.py", line 31, in main
    u_qdot = mobile_base_controller.compute_setpoint_control_signal(q, vec8(task_reference))
RuntimeError: DQ_DifferentialDriveRobot::pose_jacobian(q,to_link) only accepts to_link in {0,1}.

Expected behavior

• The compute_setpoint_control_signal() should return an array containing the control signal.

Environment:

• OS: Ubuntu 18.04
• dqrobotics version 20.4.0.26
• Python version 3.6.9

Additional context

• This issue is really similar to the last fix of issue [BUG] The pose_jacobian() of DQ_DifferentialDriveRobot is trying to access link index 3 #22 .
• The issue probably happens on line 42 of DQ_QuadraticProgrammingController.cpp when calculating the Jacobian to use in the controller. I have not tested the tracking controller, but it will probably happen on line 82 also.

[mmmarinho]

Hello, @marcos-pereira,

As you know, the DQ_DifferentialDriveRobot has a constraint in the pose_jacobian() that makes most of the controller code in CPP incompatible with it. Have you tried it in MATLAB? Could you make a minimal working example and let me know?

If it also doesn't work in MATLAB, we'll need to think of a fix for all the languages in a future version of the library and that will take longer.

Cheers,
Murilo

[marcos-pereira]

Hi @mmmarinho ,

Thanks for the note. I tried the same code on MATLAB
Code

q = [0.12 0 0];
robot = DQ_DifferentialDriveRobot(1,1);
J_pose = robot.pose_jacobian(q);

solver = DQ_QuadprogSolver;
controller = DQ_ClassicQPController(robot,solver);
gain_controller_eta = 10;
controller.set_gain(gain_controller_eta);
controller.set_damping(0.001);
controller.set_control_objective(ControlObjective.Pose);

x = 1;
y = 1;
phi = 0;
qd = [x, y, phi];
task_reference = robot.fkm(qd);

u_qdot = controller.compute_setpoint_control_signal(q,vec8(task_reference))

Output

Matrix dimensions must agree.

Error in DQ_ClassicQPController/compute_objective_function_symmetric_matrix (line
44)
             H = J'*J +
             controller.damping*eye(controller.robot.get_dim_configuration_space());
             
Error in DQ_TaskspaceQuadraticProgrammingController/compute_setpoint_control_signal
(line 73)
                H = controller.compute_objective_function_symmetric_matrix(J,
                task_error);

Error in issue_holonomic_base_jacobian (line 26)
u_qdot = controller.compute_setpoint_control_signal(q,vec8(task_reference))

• Matlab version: R2018a
• DQ robotics version: development branch

Since J is a 8x2 matrix, J'*J results in a 2x2 matrix. The damping factor generated is being generated with an identity matrix 3x3 because the dimension of the configuration space is 3. However, I am not sure if only using an identity matrix 2x2 will fix the problem because of the dimensions of the other matrices in the optimization problem.

While the DQ_DifferentialDriveRobot is not working with the QP controller, I will try to use the DQ_HolonomicBase robot model in my application. I have already adapted the holonomic_base_control.m to use the QP controller in MATLAB. I will adapt it to Python.

In the case of further testing with the DQ_DifferentialDriveRobot, just let me know.

[mmmarinho]

Hello, @marcos-pereira,

Thank you for the report. As I expected, the controller code does not currently behave well for DQ_DifferentialDriveRobot in any language.

Please let me discuss this internally with @bvadorno and we'll work on a comprehensive fix for a future release.

Your approach seems good for the time being.

Thanks again for your time.

Cheers,
Murilo

[bvadorno]

Hi @marcos-pereira,

A good way to go in this case is to use the DQ_HolonomicBase and then use an equality constraint to enforce the nonholonomic constraint, similarly to what we did in Quiroz-Omaña, Juan José, and Bruno Vilhena Adorno. 2018. “Whole-Body Kinematic Control of Nonholonomic Mobile Manipulators Using Linear Programming.” Journal of Intelligent & Robotic Systems 91 (2): 263–78. https://doi.org/10.1007/s10846-017-0713-4.

Best regards,
Bruno

[marcos-pereira]

Hello @bvadorno ,

Thanks for the idea. I will check on that.

Best regards,
Marcos