Simple Sine Movement in Python

To show the remote access to Oncilla API Level 2 we provide an example written in Python that performs an simple sine movement, by sending commands to the Simulation over the Robotics Service Bus (RSB), which is available in Python, C++, Java and Common Lisp.

In order to start the example, usew Webots to open the world file Example3.wbt which is installed by the Oncilla Project Wizard. This world file will start a controller for Oncilla, that listens to commands send over RSB.

You find the example Python script at /usr/share/cca-oncilla/examples/OncillaRemoteSimpleSineMovement.py. Start the python script:

python /usr/share/cca-oncilla/examples/OncillaRemoteSimpleSineMovement.py

The script does:

1. Importing necessary RSB and RST modules for remote communication with the Oncilla CCA Interface.
2. Creating an informer to send the commands to a specified scope /oncilla/cmd/pos/all (lines 27-28).
3. Creating a JointAngles object containing the eight joint commands (lines 52-60).
4. Sending the command with the informer created above (line 62).
5. Make sure you deactivate the informer at the end of your program.

import math
import time

import rsb
import rst

steps = 0

from rsb.converter import registerGlobalConverter, ProtocolBufferConverter

from rst.kinematics import JointAngles_pb2

def reg(klass):
    registerGlobalConverter(ProtocolBufferConverter(messageClass=klass))
reg(JointAngles_pb2.JointAngles)
rsb.__defaultParticipantConfig = rsb.ParticipantConfig.fromDefaultSources()

def hip_position(phase, amplitude, offset):
    return amplitude * math.cos(phase) + offset

def knee_position(phase, phase_lag, amplitude, offset):
    if ((phase + phase_lag) > math.pi):
        return amplitude * 1 + offset
    else:
        return amplitude * 0 + offset

if __name__ == '__main__':
    
    hip_amplitude = 30
    fore_hip_offset = 0
    hind_hip_offset = 0
    fore_knee_amplitude = 1
    fore_knee_offset = 0
    hind_knee_amplitude = 0
    hind_knee_offset = 0
    fore_hip_knee_phase_lag = 0
    hind_hip_knee_phase_lag = 0
    frequency = 3
    time_s = 0.0
    phase = 0.0
    antiphase = 0.0
    
    def sendCommand(e):
        global time_s
        global steps
        global frequency
        global phase
        global antiphase
        
        hip_amplitude = 30
        fore_hip_offset = 0
        hind_hip_offset = 0
        fore_knee_amplitude = 1
        fore_knee_offset = 0
        hind_knee_amplitude = 0
        hind_knee_offset = 0
        fore_hip_knee_phase_lag = 0
        hind_hip_knee_phase_lag = 0

        steps = steps + 1
        time_s = e.metaData.createTime
        
        phase = (time_s * frequency * 2 * math.pi) % (2 * math.pi)
        antiphase = (phase + math.pi) % (2 * math.pi)
        
        # Only print every 50th time
        if (steps%50 == 0):
            print "\x1b[1;1f\x1b[J"
            print "\033[01;40m"
            print "Step:\n", steps
            print "Time:\n", time_s
            print "Angles:\n", e.data
        
        # generate the eight desired joint angles
        a = JointAngles_pb2.JointAngles()
        a.angles.append(hip_position(phase, hip_amplitude, fore_hip_offset) / 180.0 * math.pi) # lf hip
        a.angles.append(hip_position(antiphase, hip_amplitude, fore_hip_offset) / 180.0 * math.pi) # rf hip
        a.angles.append(hip_position(antiphase, hip_amplitude, hind_hip_offset) / 180.0 * math.pi) # lh hip
        a.angles.append(hip_position(phase, hip_amplitude, hind_hip_offset) / 180.0 * math.pi) # rh hip
        a.angles.append(knee_position(phase, fore_hip_knee_phase_lag, fore_knee_amplitude, fore_knee_offset)) # lf knee
        a.angles.append(knee_position(antiphase, fore_hip_knee_phase_lag, fore_knee_amplitude, fore_knee_offset)) # rf knee
        a.angles.append(knee_position(antiphase, hind_hip_knee_phase_lag, hind_knee_amplitude, hind_knee_offset)) # lh knee
        a.angles.append(knee_position(phase, hind_hip_knee_phase_lag, hind_knee_amplitude, hind_knee_offset)) # rh knee

        send_angles.publishData(a)
    
    try:
        send_angles = rsb.createInformer("/oncilla/cmd/pos/all",
                                     dataType=JointAngles_pb2.JointAngles)
        currentangles = rsb.createListener("/oncilla/status/pos/all")
        currentangles.addHandler(sendCommand)

        while (True):
            time.sleep(1)
    
    finally:
        currentangles.deactivate()
        send_angles.deactivate() # We always have to deactivate the informer