How to run your own SCL controllers on the KUKA LWR (Hardware / No Simulation)

As was described in the Introduction you can control the KUKA LWR by starting the process kuka_torque_interface. Your controller will run in a different process on a different CPUSet and communicate with kuka_torque_interface through shared memory.

1. CPU Sets The process kuka_torque_interface is started by a shell script (see Usage) that will create the required CPUSets and start the process in the right CPUSet. Your SCL Controller Process will need to be started by a smiliar shell script, such as the following.

#!/bin/bash

# 2 of 4 CPUs for the kuka_torque_interface process
sudo cset set -c 0-3 TORQUE_INTERFACE
# 1 of 4 CPUs for your SCL controller process
sudo cset set -c 4-5 SCL_CONTROLLER
# 1 of 4 CPUs for all system processes
sudo cset set -c 6-7 SYSTEM

# Move all system processes to the SYSTEM CPU SET
sudo cset proc -m -k --force -f root -t SYSTEM

# Start your controller process in the according CPU SET 
sudo cset proc -s SCL_CONTROLLER -e ./YOUR_SCL_CONTROLLER_BINARY

2. SMIC Library Documentaion

The Shared Memory Interprocess Communication Library (SMIC) is very easy to use. Here's the general principal: A token is used to synchonize the communication between your SCL controller process and kuka_torque_interface. Right after the kuka_torque_interface received new sensor data, it will free the token, so your SCL controller process can start computing the torques. In this moment your controller process will grab the token. It will then compute the torques and write them to shared memory. When it's done it frees the token.

Here's a complete example with thorough documentation:

#!c++

#include "SMIC.hpp"

int main (void) {
  // Create an object that will have access to the same memory as kuka_torque_interface
  SMIC SharedMemory;

  //Free the token in case it should be blocked from a previous crash of the program
  SharedMemory.FreeToken();

  // Tell kuka_torque_interface that your SCL controller process was started. This will initiate the robots
  // movement from the candle position to a starting position outside of the singularity. Right now this
  // starting position is still hardcoded in kuka_torque_interface
  SharedMemory.StartControllerProcess();

  // Create float arrays with 7 Elements for storing various data from the robot
  float ExampleArrayForStoringData[7];

  // Create an array to store the computed torques for 7 joints
  float CommandedJointTorquesInNm[7];

  // Create a variable for storing the time
  // This time is set to zero by kuka_torque_interface everytime control is handed over to your SCL controller process
  int TimeInMillisec = 0;

  // Main loop with whatever terminating condition suits you
  while(1) {
    // You need to let kuka_torque_interface know that your process has not crashed for safety reasons.
    SharedMemory.CallHome();

    if(SharedMemory.MoveToStartPositionInProgress()) {
      // Don't do anything. Just wait until kuka_torque_interface moved the robot to the starting position
    }
    else {
      // Wait until kuka_torque_interface releases the token
      // This happens, once kuka_torque_interface receives new sensor data from the robot
      if (SharedMemory.GrabToken(1000000)==false) {
        //If this happens kuka_torque_interface probably crashed
      }

      // Get the time
      TimeInMillisec = SharedMemory.GetTimeInMillisec();

      // Get the data you need from the robot
      SharedMemory.GetMeasuredJointPositions(ExampleArrayForStoringData);
      SharedMemory.GetEstimatedJointVelocities(ExampleArrayForStoringData);
      SharedMemory.GetEstimatedJointAccelerations(ExampleArrayForStoringData);
      SharedMemory.GetMeasuredJointTorques(ExampleArrayForStoringData);
      SharedMemory.GetMeasuredCartesianPose(ExampleArrayForStoringData);
      SharedMemory.GetEstimatedCartesianForcesTorques(ExampleArrayForStoringData);

      //
      //  Compute joint torques in SCL and store them in CommandedJointTorquesInNm
      //

      // Write the commanded torques to shared memory
      SharedMemory.SetCommandedJointTorques(CommandedJointTorquesInNm);
      //Tell kuka_torque_interface that kuka_scl_controller is done computing the torques
      SharedMemory.FreeToken();

    }
  }

  SharedMemory.StopControllerProcess();
  SharedMemory.FreeToken();

  delete SharedMemory;

return 0;
}

If you need to know anything more specific, please have a look at SMIC.hpp

Contact: gerlinghaus@cs.stanford.edu

Wiki

scl-manips-v2 / haptic_teleoperation_demo / software

How to run your own SCL controllers on the KUKA LWR (Hardware / No Simulation)