5. Tutorial 3: Simulating via Fast Research Interface

This tutorial shows how to combine the dynamics simulation with the emulation of the robot network interface called Fast Research Interface (FRI). To accomplish the interface emulation the fripy package is used. The controller part we used in the previous tutorial controls the robot via FRI. The simulation sends back the current data of the simulated robot.

5.1. Setup of the simulation side (the robot side)

5.1.1. Initialization

First do the imports and define the settings for the simulator. Then initialize an FRIClient, which emulates the robot side of the Fast Research Interface. The controller will connect via network and we handle the simulation inputs (torques) and outputs (joint positions) using our FRIClient instance.

import time
import numpy as np

from lwrsim.lwr4_q2q4 import DynamicsSimulator
from lwrmodels.lwr4_q2q4.dynamics import get_gravity_torque_vector as tau_g
from sundials import IDAError

from fripy.fricomm import FRI_CTRL_POSITION, FRI_CTRL_JNT_IMP, LBR_MNJ
from fripy.fricomm import FRI_STATE_CMD, FRI_QUALITY_PERFECT
from fripy.client import FRIClient

# Set the sampling time
Ts = 0.01
# Set inital joint positions
q0 = [0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0]
# Set inital joint velocities
dq0 = [0.0]*LBR_MNJ
# Set absolute tolerance for the solver
abstol = 1.0e-6
# Set relative tolerance for the solver
reltol = 1.0e-5

# Now we can initialize the simulator / the ODE solver
ds = DynamicsSimulator(sampleTime=Ts, q0=q0, dq0=dq0,
                        abstol=abstol, reltol=reltol)

# Initialize the FRIClient which emulates the robot's network communication
fri = FRIClient(debug=False, verbose=True)

# As we want to allow the control on torque level
# we use the joint impedance control strategy
fri.set_control_strategy(FRI_CTRL_JNT_IMP)
# Open the interface. The communication works as fast as possible,
# so the answer time argument is not important. However, if the
# simulation takes longer than this time, a timeout occurs.
# So we use sth. larger to allow a simulation slower than real-time
fri.open(ds.sampleTime*100.0)

# Also set the inital position of the interface data
fri.msr.data.msrJntPos = ds.get_joint_positions()[:]

# Quit after a after timeout seconds with no communication
last_comm_time = time.time()
timeout = 10.0

5.1.2. Dynamics Simulation and Communication via FRI

We now obtain the actuation torques from the controller via FRI. The gravitation is compensated on this side, however. If we are in command mode, we simulate the motion dynamics for the supplied torques and then return the joint positions to the remote side via FRI.

# Simulate with torques supplied by FRI and return joint positions via FRI
while True:
    # Call fri.start (allow control) and
    # go to command mode if quality is perfect
    if (fri.msr.intf.quality is  FRI_QUALITY_PERFECT and
        fri.msr.intf.state is not FRI_STATE_CMD):
        fri.start()

    # Check that we use Joint Impedance Control strategy
    if  fri.msr.robot.control is FRI_CTRL_JNT_IMP:
        fri.msr.data.msrJntTrq = [0.0]*LBR_MNJ

        # Compute and compensate gravity torque as the real robot does
        gravpos = np.array(ds.get_joint_positions())[ds.simulatedJoints]
        gravtorque = tau_g(gravpos)

        for jnt in range(len(ds.simulatedJoints)):
            simjnt = ds.simulatedJoints[jnt]
            fri.msr.data.msrJntTrq[simjnt] += gravtorque[jnt]

        # If we are in command mode, apply the supplied torque addJntTrq
        # and simulate the motion dynamics
        # then update the joint positions in the fri.msr.data structure
        if fri.msr.intf.state is FRI_STATE_CMD:
            for jnt in range(LBR_MNJ):
                fri.msr.data.msrJntTrq[jnt] += fri.cmd.cmd.addJntTrq[jnt]

            try:
                ds.calc_next_state(fri.msr.data.msrJntTrq)
            except (IDAError):
                # Handle the solver exception
                # In most cases the supplied torques were inappropriate
                print(("Warning: Simulation failed, see error message"
                    "above."))
                # For now just tolerate this and pass
                # Here, you could also reset the simulation state
                # to the previous position, brake the robot
                # and switch to monitor mode
                # or quit directly

            fri.msr.data.msrJntPos = ds.get_joint_positions()

            last_comm_time = time.time()

    elif fri.msr.robot.control is FRI_CTRL_POSITION:
        # If position control strategy is used handle it here
        # Should not be the case as we want to control
        # the robot on torque level
        if fri.msr.intf.state is FRI_STATE_CMD:
            fri.msr.data.msrJntPos = fri.cmd.cmd.jntPos[:]
            ds.set_state(fri.msr.data.msrJntPos, [0.0]*LBR_MNJ, False)

    # Here we handle a timeout to quit our application if communication
    # failed for some time
    if time.time() > last_comm_time + timeout:
        print("Simulator closing due to timeout.")
        fri.msr.krl.intData[0] = 2
        fri.send_data()
        fri.close()
        break

    # Quit if remote side wants to stop
    if fri.cmd.krl.intData[1] == 2:
        print("Stop by server requested")
        fri.msr.krl.intData[0] = 2
        fri.send_data()
        fri.close()
        break

    # Send and receive data via FRI
    fri.send_data()
    fri.recv_data()

    # This handles a bug
    # If we receive an old package just receive again to catch up
    if fri._dont_answer:
        fri._dont_answer = False
        fri.recv_data()

5.2. Setup of the controller side (the remote side)

5.2.1. Initialization

First do the imports and define settings for the controller. Then initialize an FRIServer, which connects to the robot or the emulation (FRIClient). Also note, that it is not required to import the model or simulator on this side.

import time

import numpy as np
from numpy import pi

from fripy.server import FRIServer
from fripy.fricomm import (FRI_STATE_MON, FRI_STATE_CMD, FRI_CTRL_POSITION,
    FRI_CTRL_JNT_IMP, LBR_MNJ)


# Set the sampling time
Ts = 0.01
# Determine how many samples to simulate for 4s simulation
nsamples = int(4.0 / Ts)

# Set the controller gains
# Using same setting for all joints here
Kp = np.ones((LBR_MNJ,)) * 10.0
Ki = Kp * 0.0
Kd = 2.0 * np.sqrt(Kp) * 1.0

# Define the contant reference position
qref = np.array([0.0, 0.0, 0.0, 1.0*pi/2.0, 0.0, 0.0, 0.0])

# initialize an array to save the time to
t = np.zeros((nsamples, ))
# initialize an array to save the joint positions to
q = np.zeros((nsamples, LBR_MNJ))
# initialize an array to save the actuation torques to
tau = np.zeros((nsamples, LBR_MNJ))

# initialize arrays for
# the last position error
lasterr = np.zeros((LBR_MNJ,))
# the current position error
error = np.zeros((LBR_MNJ,))
# the integral value of the error
errorint = np.zeros((LBR_MNJ,))
# the derivative value of the error
derror = np.zeros((LBR_MNJ,))

# Initialize the FRIServer which connects
# to the robot or the emulation (FRIClient)
# supply a receive timeout
fri = FRIServer(simulation=True,timeout=1.0)

k = 0

# Define a timeout to quit the program
# if no communication is going on for some time
last_comm_time = time.time()
timeout = 10.0

5.2.2. Controlling on torque level via FRI

Then we can communicate with the robot and control it via Fast Research Interface (FRI). Note that we use the joint-impedance control strategy to allow a control on joint torque-level of the robot. The procedure is as follows:

  1. receive data from the robot

  2. compute the torque to be sent back to the robot

  3. answer the robot, i.e. send a packet to the robot (also if not in command

    mode)

  4. go to 1

In our case we quit if we have controlled the robot for a number of samples or communication is bad for some time (timeout).

while True:
    # Try to receive data
    if fri.recv_data():
        last_comm_time = time.time()
    else:
        # Quit if communication timed out
        if time.time() > last_comm_time + timeout:
            print("Quit due to communication timeout.")
            break
        # Otherwise try to receive again
        else:
            continue

    # Control the joint positions if we ware in command mode
    # limit to number of samples defined above
    if (fri.msr.intf.state is FRI_STATE_CMD) and (k < nsamples):
        # Save current data for plotting later on
        q[k,:] = fri.msr.data.msrJntPos[:]
        tau[k,:] = fri.msr.data.msrJntTrq[:]
        t[k] = k*Ts

        # Check that we use the Joint Impedance Control Strategy
        if (fri.msr.robot.control is FRI_CTRL_JNT_IMP):

            # Compute errors etc.
            error[:] = qref - q[k,:]
            errorint += error

            # Set last error to current error initially,
            # so we avoid a large initial control torque
            # resulting from the derivative term
            if k == 0:
                lasterr[:] = error[:]

            derror[:] = (error-lasterr)/Ts
            lasterr[:] = error[:]

            # Compute the torque from feedback control
            # Note: Gravity is already compensated by the robot side
            cur_tau = (Kp * error
                    + Kd * derror
                    + Ki * errorint)

            # Hand torque over to FRIServer
            fri.ctl_jnt_imp(newJntTrq=cur_tau[:], sendData=False)

        k += 1

    # Quit if communication quality decreased
    if (fri.msr.intf.state is FRI_STATE_MON) and (k > 0):
        print("Closing connection because state decreased to monitor.")
        fri.cmd.krl.intData[1] = 2
        fri.send_data()
        break

    # Quit if we have simulated for all samples
    if k >= nsamples:
        print("Closing connection because number of samples reached.")
        fri.cmd.krl.intData[1] = 2
        fri.send_data()
        break

    # Quit if Robot side / KRL program wants to stop
    if fri.msr.krl.intData[0] == 2:
        print("Simulator requested to close connection.")
        fri.cmd.krl.intData[1] == 2
        fri.send_data()
        break

    # Send data to robot side
    fri.send_data()

5.2.3. Output

As before, you can plot the results:

import pylab as P

# Plot the data (refer to matplotlib documentation)
P.subplot(2,1,1)
P.plot(t, q[:,1], 'b', label='q2')
P.plot(t[[0,-1]], [qref[1]]*2, 'b--', label='qref2')
P.plot(t, q[:,3], 'g', label='q4')
P.plot(t[[0,-1]], [qref[3]]*2, 'g--', label='qref4')
P.legend()

P.subplot(2,1,2)
P.plot(t, tau[:,1], label='tau2')
P.plot(t, tau[:,3], label='tau4')
P.legend()

P.show()

The result should look as in the first tutorial, i.e. similar to:

_images/lwrsim.png

Kp = 10, Ts = 0.01

In general, reading the data is possible on both sides. However, as you can only obtain the data via FRI from the real robot, it is reasonable to assume this for the simulation as well. This means, we can only rely on the information we get via FRI and not internal data of the simulation side such as the joint velocities.

If you set Kp to zero (no control torque), you will note that the robot does not move. This is similar to the real robot in gravity compensation mode.

Note

You can not obtain the joint velocities directly anymore, because they are not accessible via the Fast Research Interface.

5.3. Parallel setup

If you want to you can use the module LWRSimulator class. It implements the above steps of the simulation setup in parallel. This simplifies the use of the simulation and allows to do further computations in a parallel foreground process. In particular, the simulation side can then be setup as follows:

TODO

5.4. Summary

TODO