[QUESTION] Implementing QP controller in MATLAB

[AzizAlsayed]

Acceptance of the Code of Conduct

Leave these in your question, read, and check them.

• [x] I understand that support for DQRobotics is given voluntarily.
• [x] In understand that any possible response and its timeliness will be based on the relevance, accuracy, and politeness of my request and the following discussion.
• [x] If a DQRobotics member replies, I will let them know, politely, if their response solves my issue or not.

• After going through DQ Robotics Example in lesson 8, I implemented Quadratic programming controller in MATLAB.

• So, the robot was defined as seven_dof_planar_robot=SevenDofPlanarRobotDH.kinematics(); Therefore, I changed it to kuka = KukaLwr4Robot.kinematics(); As shown below

---

Please find attached the full code

clear all;
close all;
include_namespace_dq;

% Define the robot
kuka = KukaLwr4Robot.kinematics();

% Robot definition
%seven_dof_planar_robot = SevenDofPlanarRobotDH.kinematics();
% Solver definition
qp_solver = DQ_QuadprogSolver();

% Controller definition
translation_controller = DQ_ClassicQPController(kuka, qp_solver);
translation_controller.set_control_objective(ControlObjective.Translation);
translation_controller.set_gain(10);
translation_controller.set_damping(1);

% Joint limits
q_minus = -(pi/8)*ones(7,1);
q_plus  = (pi/8)*ones(7,1);

% Desired translation (pure quaternion)
td = 7*j_;
% Sampling time [s]
tau = 0.01;
% Simulation time [s]
final_time = 4;
% Initial joint values [rad]
q = zeros(7,1);
% Store the joint values
stored_q = [];

% Translation controller loop.
for time=0:tau:final_time

        % Define the linear inequality matrix and the linear inequality vector
    Wjl = [-eye(7); eye(7)];
    wjl = [-1*(q_minus-q); 1*(q_plus-q)];

     % Update the linear inequalities in the controller
    translation_controller.set_inequality_constraint(Wjl, wjl);

       % Get the next control signal [rad/s]
    u = translation_controller.compute_setpoint_control_signal(q,vec4(td));

    % Move the robot
    q = q + u*tau;

    % Store data
    stored_q = [stored_q q];

    % Plot
    % Plot the robot
    plot(kuka,q);
    title(['Translation control' ' time = ' num2str(time) 's out of ' num2str(final_time) 's'])
    % Plot the desired pose
    hold on
    plot3(td.q(2),td.q(3),td.q(4), 'ko');
    hold off
    % [For animations only]
    drawnow limitrate % [For animations only] Ask MATLAB to draw the plot now
end

for i=1:7
    subplot(3,3,i)
    plot(stored_q(i,:))
    hold on
    plot(repmat(q_minus(i),1,size(stored_q,2)),'k')
    plot(repmat(q_plus(i),1,size(stored_q,2)),'k')
    title(['q_{' num2str(i) '}'])
    ylim([-1 1])
    box off
    ylabel('[rad]')
    xlabel('Iteration')
end

---

• When I run the code I keep getting the same error for every time, but I do not think I can change this function

Environment:

• OS: [Windows 10]
• dqrobotics version (current version of the master branch)
• MATLAB R2021b

[AzizAlsayed]

Hello,

I have updated MATLAB version and everything is working now.

kind regards, Aziz

[mmmarinho]

@AzizAlsayed Thanks for writing up the detailed issue and for the solution!