Planes

[AzizAlsayed]

Acceptance of the Code of Conduct

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

• I understand that support for DQRobotics is given voluntarily.
• 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.
• If a DQRobotics member replies, I will let them know, politely, if their response solves my issue or not.

I am implementing three planes. The first plane is the end-effector plane (Yellow in MATLAB) and the other two are to prevent collusion with the robot (Blue in MATLAB). So, in MATLAB the code is working as expected, but when I implement these planes in vrep the robot is not going to the target plane.

---

Please find attached the full code

clear all;
close all;
clc;
include_namespace_dq;

% Initialize V-REP interface
vi = DQ_VrepInterface();
vi.disconnect_all();
vi.connect('127.0.0.1',19997);
vi.start_simulation();

% Planes 
pi_top = i_- E_*0.5;
pi_bottom = k_ + -E_*0;

% Define the Robot 
KukaLwr4Robot = LBR4pVrepRobot("LBR4p", vi);
kuka = KukaLwr4Robot.kinematics();
kuka.name = 'KUKA';
xbase = kuka.get_base_frame();

% Solver definition 
qp_solver = DQ_QuadprogSolver();

% Define a list of planes
plane_list = {pi_top, pi_bottom};

% Controller definition
kuka_controller = DQ_ClassicQPController(kuka, qp_solver);
kuka_controller.set_control_objective(ControlObjective.Pose);
kuka_controller.set_gain(100);
kuka_controller.set_damping(0.01);
kuka_controller.set_stability_threshold(0.0001); 
q = KukaLwr4Robot.get_q_from_vrep();


% Joint limits
q_minus = [-pi -pi -pi/2 -pi -pi/2 -pi -pi/2]';
q_plus = [pi pi pi/2 pi pi/2 pi pi/2]';


% VFI gain
eta_d = 100;

q = [0    0    0     0      0       0       0]';
KukaLwr4Robot.send_q_to_vrep(q);


% axis equal
figure
plot(kuka, q, 'nojoints');

hold on;
plot(pi_top, 'plane', 1);
hold on
plot(pi_bottom, 'plane', 1);

axis([-3, 3,-3, 3, -3, 3])

space = kuka.get_dim_configuration_space;

% Integration step
tau = 0.001;

% Store the joint values
stored_q = [];


x=[0.9;0.1;0.9];
y=[0.93;0.9;0.95;0.95];
fr = (1 + 0.5*E_*0.5*DQ(x))*normalize(DQ(y));
centre = translation(fr);
handle_frame = plot(fr, 'name', 'plane');
 
n_pil= k_;
p_pil = DQ(0);
d_pil = Adsharp(fr,(n_pil + E_*dot(p_pil, n_pil)));
handle_plane = plot(d_pil, 'plane' ,1, 'color', 'y','location', centre);
xd = fr*(i_*(1 + E_*0.5*-0.01*k_));
i = 1;
% Loop
         W = [];
         w = [];
while ~kuka_controller.system_reached_stable_region()
    Jx = kuka.pose_jacobian(q);
    x = kuka.fkm(q);
    Jt = DQ_Kinematics.translation_jacobian(Jx, x);
    t = translation(x);
    Jp_p1 = [DQ_Kinematics.point_to_plane_distance_jacobian(Jt, t, plane_list{1,1})];
    Jp_p2 = [DQ_Kinematics.point_to_plane_distance_jacobian(Jt, t, plane_list{1,2})];
    dp_p1 = DQ_Geometry.point_to_plane_distance(t, plane_list{1,1});
    dp_p2 = DQ_Geometry.point_to_plane_distance(t, plane_list{1,2});

        W = [Jp_p1;Jp_p2;-eye(7); eye(7)];
        w = [dp_p1; dp_p2; -(q_minus-q); (q_plus-q)];
       kuka_controller.set_inequality_constraint(W, w);
    xd = vi.get_object_pose('plane_list');
    u = kuka_controller.compute_setpoint_control_signal(q, vec8(xd));
    
    q = q +  u*tau;
    KukaLwr4Robot.send_q_to_vrep(q);
    vi.set_object_pose('effector', kuka.fkm(q))
    vi.set_object_pose('base', xbase);
    % Store data
    stored_q = [stored_q q];
    
    plot(kuka, q);
    
%     drawnow limitrate
pause(0.001)
         W = [];
         w = [];
end

vi.stop_simulation();
vi.disconnect()

---

Also, please find attached the coppeliasim file and a figure of the scene.

Planes_vrep.zip

Environment:

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

[juanjqo]

Hi @AzizAlsayed, thank you for engaging with the DQ Robotics community.

For your description, it is hard to understand what you are trying to do and the problem you are having. After reading your code, I could understand the context of the problem. Nevertheless, the example you provided is not running. I'm having the following output

Error using  + 
Arrays have incompatible sizes for this operation.

Error in question_7 (line 100)
    q = q +  u*tau;

Related documentation

, because the solver is not finding a feasible solution. Therefore, I do not know if the problem you have is related to the definition of the constraints or to the VREP interface.
Could you provide the minimal example using only Matlab (without VREP), the one you mentioned that is working as expected?

Best regards,

Juan

[AzizAlsayed]

Hi @juanjqo @bvadorno , thank you for your quick response.

Apologies for the misunderstanding.

I am implementing constrained planes and a target plane by using QP controller. So, the first plane is the end-effector plane which is the target plane (Yellow in MATLAB) and the other two planes constrained which are to prevent collusion with the robot (Blue in MATLAB). So, in MATLAB the code is working as expected, but when I implement these planes in vrep the robot is not going to the target plane.

Please find below the example by only using MATLAB.

---

Please find attached the full code

clear all;
close all;
clc;
include_namespace_dq;

% Planes 

pi_top = i_- E_*0.5;
pi_bottom = k_ + -E_*0;

% Define the Robot 

kuka = KukaLwr4Robot.kinematics();
kuka.name = 'KUKA';

% Solver definition 
qp_solver = DQ_QuadprogSolver();

% Define a list of planes
plane_list = {pi_top, pi_bottom};

% Controller definition
kuka_controller = DQ_ClassicQPController(kuka, qp_solver);
kuka_controller.set_control_objective(ControlObjective.Pose);
kuka_controller.set_gain(100);
kuka_controller.set_damping(0.01);
kuka_controller.set_stability_threshold(0.0001); 



% Joint limits
q_minus = [-pi -pi -pi/2 -pi -pi/2 -pi -pi/2]';
q_plus = [pi pi pi/2 pi pi/2 pi pi/2]';


% VFI gain
eta_d = 100;

q = [0    0    0     0      0       0       0]';


% axis equal
figure
plot(kuka, q, 'nojoints');

hold on;
plot(pi_top, 'plane', 1);
hold on
plot(pi_bottom, 'plane', 1);

axis([-3, 3,-3, 3, -3, 3])

space = kuka.get_dim_configuration_space;

% Integration step
tau = 0.001;

% Store the joint values
stored_q = [];


x=[0.9;0.1;0.9];
y=[0.93;0.9;0.95;0.95];
fr = (1 + 0.5*E_*0.5*DQ(x))*normalize(DQ(y));
centre = translation(fr);
handle_frame = plot(fr, 'name', 'plane');
 
n_pil= k_;
p_pil = DQ(0);
d_pil = Adsharp(fr,(n_pil + E_*dot(p_pil, n_pil)));
handle_plane = plot(d_pil, 'plane' ,1, 'color', 'y','location', centre);
xd = fr*(i_*(1 + E_*0.5*-0.01*k_));
i = 1;
% Loop
         W = [];
         w = [];
while ~kuka_controller.system_reached_stable_region()
    Jx = kuka.pose_jacobian(q);
    x = kuka.fkm(q);
    Jt = DQ_Kinematics.translation_jacobian(Jx, x);
    t = translation(x);
    Jp_p1 = [DQ_Kinematics.point_to_plane_distance_jacobian(Jt, t, plane_list{1,1})];
    Jp_p2 = [DQ_Kinematics.point_to_plane_distance_jacobian(Jt, t, plane_list{1,2})];
    dp_p1 = DQ_Geometry.point_to_plane_distance(t, plane_list{1,1});
    dp_p2 = DQ_Geometry.point_to_plane_distance(t, plane_list{1,2});

        W = [Jp_p1;Jp_p2;-eye(7); eye(7)];
        w = [dp_p1; dp_p2; -(q_minus-q); (q_plus-q)];
       kuka_controller.set_inequality_constraint(W, w);

    u = kuka_controller.compute_setpoint_control_signal(q, vec8(xd));
    
    q = q +  u*tau;

    % Store data
    stored_q = [stored_q q];
    
    plot(kuka, q);
    
%     drawnow limitrate
pause(0.001)
         W = [];
         w = [];
end

---

Also, here is the output of the code

[bvadorno]

Dear @AzizAlsayed,

I'm following up on @juanjqo's request, who kindly went through your code to try and understand what you want to do. Please be more specific in describing your problem. Otherwise, it will be hard for people to engage with your question.

For instance, describe what you are trying to do (i.e., what you are trying to control), the controller you are using, the constraints you are imposing onto the system, and how they are geometrically located in the scene. Those pieces of information should be clear without people going through your code, especially if it is not a minimal working example.

Kind regards,
Bruno

[juanjqo]

Dear @AzizAlsayed,

Thank you for providing more information. I spot some contributing factors to your simulation not working. However, since your example is not minimal, it is hard to know if there are more. Please check #85 for more details.

Consider this part of your code:

    xd = vi.get_object_pose('plane_list');
    u = kuka_controller.compute_setpoint_control_signal(q, vec8(xd));
    q = q +  u*tau;
    KukaLwr4Robot.send_q_to_vrep(q);
    vi.set_object_pose('effector', kuka.fkm(q))
    vi.set_object_pose('base', xbase);

• xd = vi.get_object_pose('plane_list'); Your scene does not contain any objects called plane_list!
• KukaLwr4Robot.send_q_to_vrep(q); Your scene has the dynamics engine enabled. You need to use set_joint_target_positions(jointnames, qtarget). Note that send_q_to_vrep(q) of the class LBR4pVrepRobot, which is the one you are using calls set_joint_positions.
  Ex:

        function send_q_to_vrep(obj,q)
            %% Sends the joint configurations to VREP
            %  >> vrep_robot = LBR4pVrepRobot("LBR4p", vi)
            %  >> q = zeros(7,1);
            %  >> vrep_robot.send_q_to_vrep(q)
            obj.vrep_interface.set_joint_positions(obj.joint_names,q)
        end

• vi.set_object_pose('effector', kuka.fkm(q)); Your scene does not contain any objects called effector!
• vi.set_object_pose('base', xbase); Your scene does not contain any objects called base!

Still, I'm not really sure what is the task you want to do. I don't know if you want to attach a plane in the end-effector, or if you want to minimize the distance between the end-effector and a target plane. Nevertheless, according to your code, you are performing a pose task.
Anyway, let's say you want to minimize the distance between the end-effector and a target plane while preventing collisions between the end-effector and other planes in the workspace. Furthermore, you want to use CoppeliaSim using the dynamics engine enabled.

(The green plane is the target. Red planes are obstacles)

Example:

clear all;
close all;
clc;

include_namespace_dq

vi = DQ_VrepInterface();
vi.disconnect_all();
vi.connect('127.0.0.1',19997);
vi.start_simulation();

try

jointnames={'LBR4p_joint1','LBR4p_joint2',...
            'LBR4p_joint3','LBR4p_joint4',...
            'LBR4p_joint5','LBR4p_joint6',...
            'LBR4p_joint7'};

    %---------------------Robot definition--------------------------%
    LBR4p_DH_theta = [0, 0, 0, 0, 0, 0, 0];
    LBR4p_DH_d = [0.200, 0, 0.4, 0, 0.39, 0, 0];
    LBR4p_DH_a = [0, 0, 0, 0, 0, 0, 0];
    LBR4p_DH_alpha = [pi/2, -pi/2, pi/2, -pi/2, pi/2, -pi/2, 0];
    LBR4p_DH_type = double(repmat(DQ_JointType.REVOLUTE,1,7));
    LBR4p_DH_matrix = [LBR4p_DH_theta;
                        LBR4p_DH_d;
                        LBR4p_DH_a;
                        LBR4p_DH_alpha
                        LBR4p_DH_type];
                
    robot = DQ_SerialManipulatorDH(LBR4p_DH_matrix);
    xbase = vi.get_object_pose('LBR4p_joint1');
    robot.set_reference_frame(xbase);
    robot.set_base_frame(xbase);
    robot.set_effector(1+0.5*DQ.E*DQ.k*0.07);
    xbase = robot.get_base_frame();
    n = robot.get_dim_configuration_space();
    qmin = [-pi -pi -pi/2 -pi -pi/2 -pi -pi/2]';
    qmax = [pi pi pi/2 pi pi/2 pi pi/2]';
    %----------------------------------------------------------------
    
    %---------------------Controller definition----------------------%
    qp_solver = DQ_QuadprogSolver();
    controller = DQ_ClassicQPController(robot, qp_solver);
    controller.set_gain(0.2);
    controller.set_damping(0.05);
    controller.set_control_objective(ControlObjective.DistanceToPlane)
    
    xplane = vi.get_object_pose('target_plane');
    plane_d = Adsharp(xplane,-k_);
    controller.set_target_primitive(plane_d);
    controller.set_stability_threshold(0.00001);
    %----------------------------------------------------------------
    %  Plane constraints
    safe_distance = 0.1;
    wall_plane  = Adsharp(vi.get_object_pose('wall_plane'),  k_); 
    floor_plane = Adsharp(vi.get_object_pose('floor_plane'), k_);
    back_plane  = Adsharp(vi.get_object_pose('back_plane'),  k_);
    planes = {wall_plane, floor_plane, back_plane};
    %----------------------------------------------------------------
   
    q =  vi.get_joint_positions(jointnames);
    T = 0.01;
    while ~controller.system_reached_stable_region()
        %----------VFIs------------------------------------
        x_pose = robot.fkm(q);
        p = translation(x_pose); 
        J_pose = robot.pose_jacobian(q); 
        J_trans = robot.translation_jacobian(J_pose,x_pose);
        
        for i=1:size(planes,2)
            Jdists(i,:) = robot.point_to_plane_distance_jacobian(J_trans, p, planes{i});
            dists(i)    = DQ_Geometry.point_to_plane_distance(p, planes{i}) -safe_distance;
        end

        A = [-Jdists; -eye(n); eye(n)];
        b = [dists'; (q-qmin); -(q-qmax)];
        controller.set_inequality_constraint(A,b);
        %-------------------------------------------------

        u = controller.compute_setpoint_control_signal(q, 0);
        q = q + T*u;
        vi.set_joint_target_positions(jointnames, q);
        vi.set_object_pose('effector', robot.fkm(q))
        vi.set_object_pose('base', xbase);
        norm(controller.get_last_error_signal())

    end
    vi.stop_simulation();
    vi.disconnect();

catch ME
    vi.stop_simulation();
    vi.disconnect();
    rethrow(ME)
end

Best regards,

Juan

minimal_example.zip

[mmmarinho]

@juanjqo This is a really good CoppeliaSim example in MATLAB, with a few adjustments I'd advise adding it to the MATLAB examples repo.

It pretty much covers all points I also spotted in the code.

---

Just in case this becomes an example in the near future, a couple of comments.

1. Remove this (or maybe describe that it's there to test different initial conditions?):

%vrep_robot.send_q_to_vrep(zeros(7,1)+0.001*randn(7,1)+0.000001*randn(7,1)); %+0.001*randn(7,1)

2. gain is not being used?

gain = 1;

3. (NOT NECESSARY FOR THIS EXAMPLE, BUT NECESSARY IN THE NEAR FUTURE)
   Maybe it's worth updating the MATLAB to have the new DQ_SerialVrepRobot so that set_target_joint_positions becomes available
   https://github.com/dqrobotics/cpp-interface-vrep/blob/master/include/dqrobotics/interfaces/vrep/DQ_SerialVrepRobot.h

[AzizAlsayed]

Dear @juanjqo,

Thank you so much for your assistance with my technical issue. I truly appreciate the time and effort you put into helping me, and I am grateful for your patience throughout the process.

Kind regards,
Aziz

[AzizAlsayed]

dear @juanjqo,

I tried to run the code but there is an error in ''LBR4p_DH_type = double(repmat(DQ_JointType.REVOLUTE,1,7));'' as shown below:

However, I also tried to use DQ class LBR4pVrepRobot for kuka but only the end-effector was moving

vrep_robot = LBR4pVrepRobot("LBR4p", vi);
robot = vrep_robot.kinematics();
xbase = robot.get_base_frame();

Kind regards,
Aziz

[juanjqo]

@AzizAlsayed, are you using the latest version of the DQ Robotics (Master branch)?

Please test the following steps:

1. Clone the dqrobotics/matlab:

git clone https://github.com/dqrobotics/matlab.git

2. Add the the folder in Matlab using Set Path

3. Run the code again

Please let me know if the problem you are having persists.

Best regards,

Juan

[bvadorno]

Dear @AzizAlsayed,

To complement @juanjqo's kind and immense help, you can easily interact with GitHub using GitHub Deskop if you are not comfortable with the command line. (This would be an alternative to Step 1 in the procedure above. You still must do Step 2 in MATLAB).

Kind regards,
Bruno

[juanjqo]

@mmmarinho thanks for your comments.

I updated the example taking into account your suggestions.