Vrep Matlab Synchronous Mode and DTTO Robot

Typically: "How do I... ", "How can I... " questions
sample
Posts: 6
Joined: 27 Mar 2017, 21:30

Vrep Matlab Synchronous Mode and DTTO Robot

Postby sample » 28 Jun 2017, 07:57

Hi everyone!

For the last 3-4 months I have been busy with developing my VREP dynamic model of DTTO robot (https://hackaday.io/project/9976-dtto-explorer-modular-robot). I have made the dynamic model and connection to matlab which works very good in asynchronous mode (code below).

Code: Select all

vrep=remApi('remoteApi');
vrep.simxFinish(-1);

clientID=vrep.simxStart('127.0.0.1',19999,true,true,5000,5);
if (clientID>-1)
    disp('Connected to remote API server');
   
    %handle
   [returnCode,male_joint]=vrep.simxGetObjectHandle(clientID,'male_joint',vrep.simx_opmode_blocking);
   [returnCode,female_joint]=vrep.simxGetObjectHandle(clientID,'female_joint',vrep.simx_opmode_blocking);
   [returnCode,male_dyn]=vrep.simxGetObjectHandle(clientID,'male_dyn',vrep.simx_opmode_blocking);
 
   
    %other
   [returnCode,position_male]=vrep.simxGetJointPosition(clientID,male_joint,vrep.simx_opmode_streaming);
   [returnCode,position_female]=vrep.simxGetJointPosition(clientID,female_joint,vrep.simx_opmode_streaming);
   [returnCode,orientation_angels]=vrep.simxGetObjectOrientation(clientID,male_dyn,-1,vrep.simx_opmode_streaming);
   
    Kp=3;
    Ki=0.1;
    int=0;
    dt=0.001;
    gamma_ref=0;
   
   tic
   while (clientID~=-1)
       
       
    time = toc
           
    w=2;
    amp=45;
    phs=180;
    a=deg2rad(amp);
    p=deg2rad(phs);
   
   
   
   [returnCode,position_male]=vrep.simxGetJointPosition(clientID,male_joint,vrep.simx_opmode_buffer);
   [returnCode,position_female]=vrep.simxGetJointPosition(clientID,female_joint,vrep.simx_opmode_buffer);
   [returnCode,orientation_angels]=vrep.simxGetObjectOrientation(clientID,male_dyn,-1,vrep.simx_opmode_buffer);
   
   %PID controller
   gamma=orientation_angels(1,2)
   e=gamma_ref-gamma;
   int=int+e*dt;
   c=Kp*e+Ki*int;
   
   
   
    y=(a*sin((w*time)+p+(p/2))+c)
    x=(a*sin((w*time)+p)+c)
        [returnCode]=vrep.simxSetJointTargetPosition(clientID,male_joint,y,vrep.simx_opmode_blocking);
        [returnCode]=vrep.simxSetJointTargetPosition(clientID,female_joint,x,vrep.simx_opmode_blocking);
   
   
   end
   vrep.simxFinish(-1);
end

vrep.delete();


As you can see, I am trying to implement PID controller and therefore I should use Synchronous Vrep - Matlab connection. I have managed to establish the synchronous connection but it is very slow and it is not working properly. I have read all the suggested topics for the similar problems but I think this problem is unique because I need time to run the motors in sinusoidal manner ( as you can see I am using tic/toc in asynchronous mode).

My question is how to solve the 'time' problem in synchronous mode, or to be more precise what 'time' should I use and how?
Where to put synchronous triggers to get fluid simulation? How do I use sync triggers when I already have control loop? I am not very good with programming so feel free to edit the code if you think you know the solution.
There is no problem with the scene, because in asynchronous mode simulation is fluid!

I want to thank the guys working on dtto project for all the help, and thank you in advance for your input!

coppelia
Site Admin
Posts: 6058
Joined: 14 Dec 2012, 00:25

Re: Vrep Matlab Synchronous Mode and DTTO Robot

Postby coppelia » 30 Jun 2017, 13:40

Hello,

have a look at the model Models/tools/simulation stepper tool.ttm and the customization script it has attached. By looking at how individual simulation steps are triggered, you could use a similar code to trigger each simulation step, from your remote API client application. This represents an alternative way of running your remote API client application synchronously with a simulation.
But, simulation speed is of course slower, since there is the communication overhead and the waiting.

Cheers

sample
Posts: 6
Joined: 27 Mar 2017, 21:30

Re: Vrep Matlab Synchronous Mode and DTTO Robot

Postby sample » 14 Jul 2017, 18:02

Hi, I have managed to get fairly good simulation in sync mode using the following Matlab code:

Code: Select all

   
    disp('Program started');
    vrep=remApi('remoteApi');
    vrep.simxFinish(-1);
    clientID=vrep.simxStart('127.0.0.1',19997,true,true,5000,5);

    if (clientID>-1)
        disp('Connected to remote API server');

        vrep.simxSynchronous(clientID,true);
        vrep.simxStartSimulation(clientID,vrep.simx_opmode_blocking);
       

 %handle
   [returnCode,male_joint]=vrep.simxGetObjectHandle(clientID,'male_joint',vrep.simx_opmode_blocking);
   [returnCode,female_joint]=vrep.simxGetObjectHandle(clientID,'female_joint',vrep.simx_opmode_blocking);
   [returnCode,male_dyn]=vrep.simxGetObjectHandle(clientID,'male_dyn',vrep.simx_opmode_blocking);
 
   
 %other
   [returnCode,position_male]=vrep.simxGetJointPosition(clientID,male_joint,vrep.simx_opmode_streaming);
   [returnCode,position_female]=vrep.simxGetJointPosition(clientID,female_joint,vrep.simx_opmode_streaming);
   [returnCode,orientation_angels]=vrep.simxGetObjectOrientation(clientID,male_dyn,-1,vrep.simx_opmode_streaming);
   [returnCode,positions]=vrep.simxGetObjectPosition(clientID,male_dyn,-1,vrep.simx_opmode_streaming);
   
 %PI parameters
    Kp=0.5;
    Ki=0;
    int=0;
    dt=0.05;
    psi_ref=0;
   
time = [];
x=[];
y=[];
z=[];
psi = [];
e=[];
c=[];
pm=[];
pf=[];
k=1;
   
%Actuation parameters     
    w_m=3;
    w_f=3;
    amp_m=60;
    amp_f=60;
    phs=180;
    phs_s=75;
    p_s=deg2rad(phs_s);
    a_m=deg2rad(amp_m);
    a_f=deg2rad(amp_f);
    p=deg2rad(phs);
   
   
   while (clientID~=-1)
   
    time(k)=vrep.simxGetLastCmdTime(clientID)/1000; % time in [s]
     
   
   [returnCode,position_male]=vrep.simxGetJointPosition(clientID,male_joint,vrep.simx_opmode_buffer);
   [returnCode,position_female]=vrep.simxGetJointPosition(clientID,female_joint,vrep.simx_opmode_buffer);
   [returnCode,orientation_angels]=vrep.simxGetObjectOrientation(clientID,male_dyn,-1,vrep.simx_opmode_buffer);
   [returnCode,positions]=vrep.simxGetObjectPosition(clientID,male_dyn,-1,vrep.simx_opmode_buffer);
   
   %Positions
   x(k)=positions(1,1);
   y(k)=positions(1,2);
   
   %PID
   psi(k)=orientation_angels(1,2);
   e(k)=psi_ref-psi(k);
   int=int+e(k)*dt;
   c(k)=Kp*e(k)+Ki*int;
   
   
   %Actuation
    pm(k)=(a_m*sin(((w_m-c(k))*time(k))+p+(p_s)));
    pf(k)=(a_f*sin(((w_f-c(k))*time(k))+p));
        [returnCode]=vrep.simxSetJointTargetPosition(clientID,male_joint,pm(k),vrep.simx_opmode_blocking);
        [returnCode]=vrep.simxSetJointTargetPosition(clientID,female_joint,pf(k),vrep.simx_opmode_blocking);
   
    k=k+1;
    vrep.simxSynchronousTrigger(clientID);
    vrep.simxGetPingTime(clientID);
 
   end
   vrep.simxFinish(-1);

 
        vrep.simxStopSimulation(clientID,vrep.simx_opmode_blocking);
        vrep.simxFinish(clientID);
       
    else
        disp('Failed connecting to remote API server');
    end
    vrep.delete(); % call the destructor!
   
    disp('Program ended');


It takes approximately 2.5 minutes in real time for 1 minute of simulation dt=50ms.
I have read a lot about the topic and, if I am not wrong, I should use dt=5ms when using remote API (Matlab). When I use dt=5ms simulation is too slow, as a matter of fact it is so slow it's useless.

My question is:
Is it ok to use dt=50ms, and what are the consequences?
If it is not ok, how can I speed up the simulation? I have looked at the suggested code but I don't get it. I believe the problem lies in the fact I'm using time for actuation of the joints... Is there a way to actuate joints in sinusoidal fashion without using time as a variable?

On the following link you can get the scene I am working on:

https://drive.google.com/open?id=0B4xcG ... 3FVcEhiYzg

Thank you for your help!

coppelia
Site Admin
Posts: 6058
Joined: 14 Dec 2012, 00:25

Re: Vrep Matlab Synchronous Mode and DTTO Robot

Postby coppelia » 16 Jul 2017, 20:01

You do not need to use a simulation time step of 5ms. But if you want to precisely control a dynamically enabled joint from an external program, then you should use the same time step as the physics engine is using, and run in synchronous mode. Otherwise the control will not be accurate. Make sure to carefully read this page for additional details.

When using a smaller simulation time step, then all your scripts will also run more often. And all sensors, etc. will also be handled more often. If you however explicitely handle your sensors, you can make sure that they will only be handled when you need the result, and so avoid spending too much time with them.

In any case, it is important to indentify the bottleneck, which is responsible for making things slow, before thinking about a solution to making things faster.

Cheers

sample
Posts: 6
Joined: 27 Mar 2017, 21:30

Re: Vrep Matlab Synchronous Mode and DTTO Robot

Postby sample » 17 Jul 2017, 09:22

So, if I want to be sure my remoteAPI PI controller is working (or not working) I have to use 5ms time step otherwise I might get strange results for which i won't be able to determine the cause. OK, I get that.

I have identified the bottleneck, but I don't know how to bypass it - the main problem lies in following:

I am running while (clientID~=-1) loop where I am using "time=vrep.simxGetLastCmdTime(clientID)/1000" to get simulation time (in seconds - therefore /1000) in Matlab. I want to actuate two joints in sinusoidal fashion (position control) using the "vrep.simxSetJointTargetPosition(clientID,male_joint,pm(k),vrep.simx_opmode_blocking);" where pm is "pm=(a_m*sin(((w)*time)+p));" standard sine formula. I believe that this sin(w*t) calculation is causing the problem because I am sending the new position information for every little moment of time. I have removed all the redundancy from the code (everything except position control) and the simulation is still slow, that's why I am sure this is causing the problem. Maybe the solution is to give the joint just the two 'extreme' positions (amplitudes, let's say 75 degrees) and make it oscillate between the two (but in even fashion)? But how do I do that if I want to keep A*sin(wt) form which I need for the control of actuation? Is it possible to read let's say just 3 values for one sine cycle and feed these value into position control ''simxSetJointTargetPosition" and still get fluid joint motion?

Cheers!

coppelia
Site Admin
Posts: 6058
Joined: 14 Dec 2012, 00:25

Re: Vrep Matlab Synchronous Mode and DTTO Robot

Postby coppelia » 17 Jul 2017, 20:11

If possible, do not operate in synchronous mode and put all the low-level joint control on the V-REP side, inside of a joint control callback script. And do all the high-level stuff on the remote API client side.
Finally, try also to avoid as much as possible blocking function calls on the remote API client side.

Cheers

sample
Posts: 6
Joined: 27 Mar 2017, 21:30

Re: Vrep Matlab Synchronous Mode and DTTO Robot

Postby sample » 18 Jul 2017, 09:47

Is it possible to do the position control of the joint inside of a joint control callback script, or just apply torque and velocity? Because I am not interested in torque/velocity control but just a position. Or, is it possible to implement behavior of the joint (position) inside a VREP and call it from Matlab for every loop pass? Than I would just use a Matlab for calculation of the control signal of PID implemented in Matlab and send it to VREP where I can apply it on A, w or p, of the A*sin(wt+p) position signal for the actuators which is my prime objective.

coppelia
Site Admin
Posts: 6058
Joined: 14 Dec 2012, 00:25

Re: Vrep Matlab Synchronous Mode and DTTO Robot

Postby coppelia » 20 Jul 2017, 10:46

From how I see things, your joints should be in position control mode, and you should only submit the desired position and desired force/torque. This does not have to happen in a synchronous fashion.

I would:

  • set all joints into force/torque mode, enable the motor, enable the control loop and use the PID controller.
  • Enable the real-time mode of the simulation (the toolbar button)
  • From Matlab, regularly update the desired joint positions with simxSetJointTargetPosition.
    Call that function in a non-blocking fashion

Cheers

sample
Posts: 6
Joined: 27 Mar 2017, 21:30

Re: Vrep Matlab Synchronous Mode and DTTO Robot

Postby sample » 24 Jul 2017, 11:05

Thank you for your help.

I have given up on Matlab/VREP connection and decided to implement everything inside the VREP to bypass these problems. Now everything is working as expected.
I have one last question. In my code i have two variables 'e' and 'c', can you tell me how to plot them on the same graph? I have searched for similar problems but i think most of them are oriented on remote API signals. I know how to set graph to various user data and handle it explicitly, but I don't know how to get these variables on the graph.

Cheers

coppelia
Site Admin
Posts: 6058
Joined: 14 Dec 2012, 00:25

Re: Vrep Matlab Synchronous Mode and DTTO Robot

Postby coppelia » 25 Jul 2017, 20:55

In order to display 2 curves in the same graph, you first need to prepare two user-defined streams for a given graph. Make sure the graph is explicitely handled. Then, each time you want to add a point for the two variables, use something like:

Code: Select all

    simSetGraphUserData(graphHandle,'dataStreamA',varA)
    simSetGraphUserData(graphHandle,'dataStreamB',varB)
    simHandleGraph(graphHandle,theTime)


Make also sure to check this topic.

Cheers


Return to “General questions”

Who is online

Users browsing this forum: Baidu [Spider], rose and 21 guests