Python and ZeroMQ remote API

Typically: "How do I... ", "How can I... " questions
Post Reply
kercos
Posts: 11
Joined: 07 Jul 2022, 08:51

Python and ZeroMQ remote API

Post by kercos »

Dear Coppelia Team,

I read from https://www.coppeliarobotics.com/helpFi ... erview.htm that the legacy remote API is going to be deprecated as per CoppeliaSim V4.4, in favor of ZeroMQ remote API.

We have used the legacy API in the past with python, and we are currently trying to convert the code to ZeroMQ. However, we noticed that the performance is much worse with ZeroMQ. In particular, when moving a robot with 16 joints and sending the command via "setJointTargetPosition" this makes the movement very jittering: it takes 18 second to go through a 50 position trajectory (with no delay being specify within the loop). With the LegacyAPI the same trajectory is much smoother and we used to explicitly add a 50 ms delay in the loop to simulate a real-time movement.

One workaround we found is to use the "step-mode" in ZeroMQ, but we wonder why there is such a slow performance in non-step mode.

We also noticed that all python examples you provide which ZeroMQ API heavily rely on scripts in the scenes. We wonder if this is the only way to have smooth trajectory in non-step mode.

Thank you for your help

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

Re: Python and ZeroMQ remote API

Post by coppelia »

Hello,

did you try to use the asynchronous version of the ZeroMQ remote API? e.g. the demo scene zmqRemoteApi/clients/python/simpleTest-nonBlocking.py
In general, you want to reduce the number of times a CoppeliaSim command is called. So typically, if you need to set the joint position of 50 joints, instead of calling 50 times CoppeliaSim, it is much better practice to call CoppeliaSim a single time, via sim.callScriptFunction. That will of course require you to have a simple script function on the CoppeliaSim side that handles the task, e.g.

Code: Select all

function setJointsAtOnce(jointHandles,jointAngles)
    for i=1,#jointHandles,1 do
        sim.setJointTargetPosition(jointHandles[i],jointAngles[i])
    end
end
Cheers

kercos
Posts: 11
Joined: 07 Jul 2022, 08:51

Re: Python and ZeroMQ remote API

Post by kercos »

Thank you a lot for the quick reply!

In the old legacy api we were using simxPauseCommunication to group a number of commands that were sent in one go, and that seemed to work great! I wonder why the new API don't include a mechanism like this.

In any case, it looks like using simple callScriptFunction is a good workaround since performances are indeed much better, thanks for the tip.

I'm currently trying to familiarize myself with python child scripts (non-threaded) and noticed that sometimes they return None even though the object they are returning is not None.

To replicate the problem, try to add a python child script to the blueArm node of movementViaRemoteAPI scene with the following function:

Code: Select all

# python
def get_joints_ids():
    j_ids = [
        sim.getObject('/joint', {'index':i})
        for i in range(6)
    ]
    return j_ids

def get_joint_positions():
    print('Invoking get_joint_positions')
    j_ids = get_joints_ids()
    pos = [
        sim.getJointPosition(id)
        for id in j_ids
    ]
    print(pos)
    return pos
and run the following python test function:

Code: Select all

# python

def test():
    client = RemoteAPIClient()     
    sim = client.getObject('sim') 
    sim.startSimulation()
    pos = sim.callScriptFunction(
        'get_joint_positions@/blueArm', 
        sim.scripttype_childscript
    )
    print(pos)
    sim.stopSimulation()    
I'm not sure if I'm doing anything wrong, but although most of the time I get a list with 6 zeros (as expected), sometimes I get None. But in coppelia log I always see "Invoking get_joint_positions" followed by the list with 6 zeros.

Thanks for your help!

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

Re: Python and ZeroMQ remote API

Post by coppelia »

Hello,

although I cannot reproduce your problem, I guess the following is happening:

When calling sim.startSimulation, simulation will not be started yet at the return of that call: instead, the API function sim.startSimulation simply instructs CoppeliaSim to start the simulation as soon as possible. So by the time you try to call the script function, the script maybe hasn't initialized yet.

You have several ways to handle this situation, e.g.:
  • use a customization script instead of a child script: a customization script is initialized also when simulation is not running
  • have your child script set a signal when it ran its init function (e.g. with sim.setInt32Signal)
  • make sure that simulation time is no 0 and simulation is not stopped anymore, e.g.:

Code: Select all

...
sim.startSimulation()
while sim.getSimulationTime()==0 or sim.getSimulationState()==sim.simulation_stopped:
    time.sleep(0.01)
...
Cheers

kercos
Posts: 11
Joined: 07 Jul 2022, 08:51

Re: Python and ZeroMQ remote API

Post by kercos »

Hi,
thanks again for your support.
I've tried the last one and I still think something is off because sometimes I get None after the correct list (in the same execution). Please try this slight variation of the script:

Code: Select all

# python

def test():
    client = RemoteAPIClient()     
    sim = client.getObject('sim') 
    sim.startSimulation()
    while sim.getSimulationTime()==0 or sim.getSimulationState()==sim.simulation_stopped:
        time.sleep(0.01)
    for _ in range(10):
        time.sleep(1)
        pos = sim.callScriptFunction(
            'get_joint_positions@/blueArm', 
            sim.scripttype_childscript
        )
        print(pos)
    sim.stopSimulation()	
This is an example of what I get in the terminal after invoking the script:

Code: Select all

None
[0, 0, 0, 0, 0, 0]
None
None
None
[0, 0, 0, 0, 0, 0]
[0, 0, 0, 0, 0, 0]
[0, 0, 0, 0, 0, 0]
[0, 0, 0, 0, 0, 0]
None
And this is what I get in coppelia terminal:

Code: Select all

Invoking get_joint_positions
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Invoking get_joint_positions
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Invoking get_joint_positions
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Invoking get_joint_positions
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Invoking get_joint_positions
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Invoking get_joint_positions
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Invoking get_joint_positions
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Invoking get_joint_positions
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Invoking get_joint_positions
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Invoking get_joint_positions
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

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

Re: Python and ZeroMQ remote API

Post by coppelia »

Please try with this CoppeliaSim script:

Code: Select all

#python

def sysCall_thread():
    sim.setThreadAutomaticSwitch(True)
    while sim.getThreadExistRequest() == False:
        sim.handleExtCalls()

def get_joints_ids():
    j_ids = [
        sim.getObject('/joint', {'index':i})
        for i in range(6)
    ]
    return j_ids

def get_joint_positions():
    print('Invoking get_joint_positions')
    j_ids = get_joints_ids()
    pos = [
        sim.getJointPosition(id)
        for id in j_ids
    ]
    print(pos)
    return pos
See also the differences between Python and Lua in CoppeliaSim. Keep however in mind that using Python there is not as efficient as using Lua instead, e.g.:

Code: Select all

function sysCall_init()
end

function get_joints_ids()
    local j_ids={}
    for i=1,6,1 do
        j_ids[i]=sim.getObject('/joint',{index=i-1})
    end
    return j_ids
end

function get_joint_positions()
    print('Invoking get_joint_positions')
    j_ids = get_joints_ids()
    local pos={}
    for i=1,#j_ids,1 do
        pos[i]=sim.getJointPosition(j_ids[i])
    end
    print(pos)
    return pos
end
Also, a detail, you should use a relative joint path to get the joint handles, otherwise you might get the handles of another robot.

Cheers

kercos
Posts: 11
Joined: 07 Jul 2022, 08:51

Re: Python and ZeroMQ remote API

Post by kercos »

Thank you very much again!

Adding sysCall_thread() function does seem to solve the problem!

But does that mean this would work only on threaded python script (as opposed to non-threaded)?

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

Re: Python and ZeroMQ remote API

Post by coppelia »

Currently, yes. It seems there is a subtle bug that depends on timing in that situation...

Cheers

Post Reply