dgm_head

WARNING: Do not use this on robots other than blmc robots for safety reasons.

Dynamic Graph Head

This tutorial outlines how to program a robot using dynamic graph manager and what is called dynamic graph head (DGH). With DGH it's possible to write controllers fully in python.

The idea behind DGH is to give the minimal tools for python to talk with the dynamic graph manager control process. Such a minimal example looks like this:

import time

from robot_properties_solo.solo8wrapper import Solo8Config

import dynamic_graph_manager_cpp_bindings

# Create the dgm communication to the control process.
head = dynamic_graph_manager_cpp_bindings.DGMHead(Solo8Config.dgm_yaml_path)


D = 0.05
dt = 0.001
next_time = time.time() + dt

# In your control loop:
while (do_control):
    if time.time() >= next_time:
        next_time += dt

        ###
        # Get the latest measurements from the shared memory.
        head.read()

        ###
        # A very simple example D controller.

        # Read the joint velocities.
        joint_velocities = head.get_sensor('joint_velocities')

        # Set the joint torques.
        tau = -D * joint_velocities
        head.set_control('ctrl_joint_torques', tau)

        ###
        # Write the results into shared memory again.
        head.write()

    time.sleep(0.0001)

Starting your dgh code

At the point of writing, the blmc main DG application has to be started as root user. Therefore, also the python script needs to be started as root user to get access to the shared memory.

Steps to start a script might look like this then:

1. In a first terminal, start the main dgm program for your robot

2. Run calibration or other setup procedures.

3. Do not start the DG process. This is as only one control process should be active at a single time and we intend to use DGH

4. In a new terminal, do the following

$ sudo -s # Stay root user
$ source ~/workspace/install/setup.bash
$ source /opt/openrobots/setup.bash
$ ipython
# From within ipython, run your script then using `run your_script.py`

Putting the control in a thread

If you want to have an interactive prompt from ipython, it's necessary to run the controller off the main thread in a different thread.

Here is a full example how to do this for solo8:

import numpy as np
np.set_printoptions(suppress=True, precision=3)

import time
import threading

from robot_properties_solo.config import Solo8Config
import dynamic_graph_manager_cpp_bindings

class ZeroTorquesController:
    """A default/dummy controller that applies zero torques at the joints"""

    def __init__(self, thread):
        # Zero the commands.
        self.tau = np.zeros(12)

    def warmup(self, thread):
        self.counter = 0

    def run(self, thread):
        head = thread.head

        # Every two seconds, print the current slider positions.
        if self.counter % 2000 == 0:
            print(head.get_sensor('slider_positions'))
        self.counter += 1

        # Example for sending torques.
        head.set_control('ctrl_joint_torques', self.tau)


class DGMHeadThread(threading.Thread):
    def __init__(self, head):
        super(DGMHeadThread, self).__init__()
        self.head = head
        self.active = True

    def switch_controller(self, controller):
        controller.warmup(self)
        self.active_controller = controller

    def run(self):
        head = self.head
        self.switch_controller(ZeroTorquesController(self))

        dt = 0.001
        next_time = time.time() + dt
        while self.active:
            if time.time() >= next_time:
                next_time += dt

                # Get the latest measurements.
                head.read()

                # Run the controller.
                self.active_controller.run(self)

                # Write back the data.
                head.write()

            time.sleep(0.0001)


# Create the dgm communication.
head = dynamic_graph_manager_cpp_bindings.DGMHead(Solo8Config.dgm_yaml_path)

# Create the processing thread.
thread_head = DGMHeadThread(head)

# Start the control thread.
thread_head.start()

Reading data from vicon

To read vicon data from python, you can use the pybind11 bindigns to our vicion library. The following python code shows how to use it:

from vicon_sdk_cpp import ViconClient, ViconFrame

class ViconObject:
    """Abstraction around the ViconSDK to read position and velocity"""
    def __init__(self, robot_vicon_name):
        self.robot_vicon_name = robot_vicon_name
        self.robot_object_name = "{}/{}".format(self.robot_vicon_name, self.robot_vicon_name)

        self.client = ViconClient()
        self.client.initialize("172.24.117.119:801")
        self.client.run()

        self.frame = ViconFrame()

        self.bias_xy = np.zeros(2)

    def update(self):
        """ Call this method once every control loop to get the latest measurements. """
        self.client.get_vicon_frame(self.robot_object_name, self.frame)

    def get_state(self):
        """ Returns the position and velocity of the tracked object. """
        pos = self.frame.se3_pose.copy()
        pos[:2] -= self.bias_xy
        return pos, self.frame.velocity_body_frame.copy()

    def bias_position(self):
        """ Sets the current x/y position as zero. """
        self.bias_xy = self.frame.se3_pose[:2].copy()

Simulating Solo Robot

For simulating a Solo robot (like Solo8 in the example below), the following code might be useful:

import numpy as np
from bullet_utils.env import BulletEnvWithGround
from robot_properties_solo.solo8wrapper import Solo8Config, Solo8Robot

# Create the simulation environment with a ground.
env = BulletEnvWithGround()

# Create a robot instance and add it to the simulation.
robot = Solo8Robot()
env.add_robot(robot)

# A class to simulate the DG Head with the simulator.
class SoloSimHead:
    def __init__(self, env, robot):
        self._env = env
        self._robot = robot
        
        # Define the sensor values.
        self._sensor_joint_positions = np.zeros(8)
        self._sensor_joint_velocities = np.zeros(8)
        self._sensor_slider_positions = np.zeros(4)
        
        self._sensor_slider_positions = np.zeros(4)
        
        self._sensor_imu_gyroscope = np.zeros(3)
        
        # Utility for vicon class.
        self._sensor__vicon_base_position = np.zeros(7)
        self._sensor__vicon_base_velocity = np.zeros(6)
        
        # Controls.
        self._control_ctrl_joint_torques = np.zeros(8)
        
    def read(self):
        q, dq = self._robot.get_state()
        self._sensor_joint_positions[:] = q[7:]
        self._sensor_joint_velocities[:] = dq[6:]
        
        for i, l in enumerate(['a', 'b', 'c', 'd']):
            self._sensor_slider_positions[i] = robot.get_slider_position(l)
        
        self._sensor_imu_gyroscope[:] = dq[3:6]
        
        self._sensor__vicon_base_position[:] = q[:7]
        self._sensor__vicon_base_velocity[:] = dq[:6]
        
    def write(self):
        self._robot.send_joint_command(self._control_ctrl_joint_torques)
        self._env.step(sleep=False)
        
    def get_sensor(self, sensor_name):
        return self.__dict__['_sensor_' + sensor_name]
    
    def set_control(self, control_name, value):
        self.__dict__['_control_' + control_name][:] = value
        
    def reset_state(self, q, dq):
        self._robot.reset_state(q, dq)

# Instantiate the head class and use it similar to the DG head you get
# on the real robot.
head = SoloSimHead(env, robot)