Rock

the Robot Construction Kit

Simulate a Robot

Abstract

This tutorial is the basis for the followup tutorials in this section. Before you try this tutorial, you should have worked through the basic tutorials.

In this tutorial, we will create a library for simulating a ‘Rock’-Robot. Further we will wrap it into an orocos task and fire it up.

Implementation

Create a new library

We start by entering the tutorials folder and creating a library named ‘rock-tutorial’ by using the ‘rock-create-lib’ command.

cd ~/dev/tutorials
rock-create-lib rock_tutorial

The required dependencies for our library are ‘base/types’ and ‘gui/vizkit3d’. See the “Creating libraries” tutorial for a precise explanation of adding dependencies to a Rock library.

The automatically generated dummy files can be removed as explained in the note of the “Creating libraries” tutorial.

This should give us an folder containing the following files:

CMakeLists.txt  INSTALL  LICENSE  manifest.xml  README  src

Your src/CMakeLists.txt text file should be updated as follows:

rock_library(rock_tutorial
  SOURCES RockControl.cpp
  HEADERS RockControl.hpp
  DEPS_PKGCONFIG base-types)

Afterwards, create a new class in the src folder named ‘RockControl’, i.e. RockControl.cpp and RockControl.hpp.

This class will contain the (very basic) simulation logic for our Rock-Robot. Therefore it will calculate new positions of our robot given movement commands and a time step.

This is the content of the header file (RockControl.hpp):

#ifndef ROCKCONTROL_H
#define ROCKCONTROL_H

#include <base/commands/Motion2D.hpp>
#include <base/samples/RigidBodyState.hpp>

namespace rock_tutorial {

class RockControl
{

public:
    RockControl();
    virtual ~RockControl();
    
    base::samples::RigidBodyState computeNextPose(const double &deltaTime, 
      const base::commands::Motion2D &command);

private:
    /**
    * Wraps angles into the interval between -PI and PI.
    */
    void constrainAngle(double& angle);
    
    /**
    * This method constrains the relative rotation and
    * translation velocities of a 2D motion command.
    * Rotation should be clamped to the interval between -PI and PI.
    * Translation should be clamped to the interval between -10 and 10.
    */
    void constrainValues(base::commands::Motion2D& motionCommand);
    
    /**
    * Current Position and orientation of the rock
    **/	
    base::samples::RigidBodyState currentPose;

    /**
    * Current heading of the rock
    **/	
    double currentHeading;

    /**
    * Current speed of the rock
    **/	
    double currentRoll;

};

}

#endif // ROCKCONTROL_H

In the source file (RockControl.cpp), include the header file and declare the namespace rock_tutorial.

The constructor and destructor should look as follows:

RockControl::RockControl() {
  currentHeading = 0.0;
  currentRoll = 0.0;
  currentPose.position = base::Vector3d(0,0,0);
}

RockControl::~RockControl() {
}

The comments in the header file should help you to implement the functionality of the methods constrainAngle and constrainValues.

The type base::commands::Motion2D contains a demanded translation and a rotation speed. Translation is measured in m/s and rotation in rad/s. The parameter deltaTime defines how much time advanced since the last call. The function computeNextPose computes the new position and and rotation of our rock, in respect to the last pose.

base::samples::RigidBodyState RockControl::computeNextPose(
  const double &deltaTime, 
  const base::commands::Motion2D &inputCommand)
{
  //limit the input values. 
  base::commands::Motion2D command = inputCommand;
  constrainValues(command);

  //calculate displacement
  double delta_translation  = command.translation * deltaTime;
  double delta_rotation  = command.rotation * deltaTime;

  //apply new displacement to rock state
  currentHeading += delta_rotation;
  currentRoll += delta_translation * -2;

  //wrap angles into interval [-PI, PI]
  constrainAngle(currentHeading);
  constrainAngle(currentRoll);

  //calculate new absolute values for position and orientation
  currentPose.position += 
    Eigen::AngleAxisd(currentHeading, Eigen::Vector3d::UnitZ()) 
    * Eigen::Vector3d(0, delta_translation, 0);
  currentPose.orientation = 
    Eigen::AngleAxisd(currentHeading, Eigen::Vector3d::UnitZ()) 
    * Eigen::AngleAxisd(currentRoll, Eigen::Vector3d::UnitX());
  currentPose.orientation.normalize();

  return currentPose;
}

Finally, you can build your library using amake.

Wrapping it up

So, now that we are equipped with our library, we can go to the second step and wrap the code into an orocos task. Therefore, we create a new orocos component. Again, we use the command ‘rock-create-orogen’.

cd ~/dev/tutorials/orogen/
rock-create-orogen rock_tutorial

Define task

Again we start by adding the build dependencies in the mainfest.xml. In this case, we only depend on ‘tutorials/rock_tutorial’, as ‘rock_tutorial’ already depends on ‘base/types’ and the dependencies are resolved recursively.

In rock_tutorial.orogen, we will define the orocos task. For this tutorial, we replace the existing entries by our definitions:


name "rock_tutorial"
version "0.1"

import_types_from "rock_tutorialTypes.hpp"
import_types_from "base"
using_library "rock_tutorial"

task_context "RockTutorialControl" do
  # Declare input port motion_command
  input_port "motion_command", "base::commands::Motion2D"
  # Declare output port pose
  output_port "pose", "base::samples::RigidBodyState"
  # Set runtime behaviour
  periodic(0.01)
end

The task RockTutorialControl has an input port called ‘motion_command’ of type base::commands::Motion2D and an output port called ‘pose’ of type base::samples::RigidBodyState. This task will compute a new position and orientation each time the update hook will triggered, given the translation and rotation speed it receives on its input port. The latest motion command will be used if there is no new one. The update hook of this task will be triggered periodically.

As with all oroGen packages, we call ‘rock-create-orogen’ to finalize the package creation, and can then write the component implementation in tasks/RockTutorialControl.hpp and tasks/RockTutorialControl.cpp. You will need the following attributes:

RockControl* control;
double taskPeriod;

These need to be declared in the header file and initialized in the constructors of the source file.

The implementation of the startHook and the updateHook is simple, as the task only calls the library and passes on the result.

bool RockTutorialControl::startHook()
{
  //delete last instance in case we got restarted
  if(control)
    delete control;

  //create instance of the controller
  control = new RockControl();

  //figure out the period in which the update hook get's called
  taskPeriod = TaskContext::getPeriod();

  return RockTutorialControlBase::startHook();
}

void RockTutorialControl::updateHook()
{    
  //read new motion command if available
  base::commands::Motion2D motionCommand;
  _motion_command.readNewest(motionCommand);

  //compute new position based on the input command
  base::samples::RigidBodyState rbs = 
    control->computeNextPose(taskPeriod, motionCommand);
 
  //set time stamp 
  rbs.time = base::Time::now();

  //write pose on output port
  if(_pose.connected())
    _pose.write(rbs);
}

Run it

Now we should run it to see if it works. Create a scripts folder and create a new rockTutorial1.rb script containing:

require 'orocos'
require 'readline'
include Orocos

## Initialize orocos ##
Orocos.initialize

## load and add the 3d plugin for the rock
Orocos.run 'rock_tutorial::RockTutorialControl' => 'rock_tutorial_control' do


  rockControl = Orocos.name_service.get 'rock_tutorial_control'    

  ## Create a sample writer for a port ##
  sampleWriter = rockControl.motion_command.writer
  
  ## Start the tasks ##
  rockControl.start
  
  ## Write motion command sample ##
  sample = sampleWriter.new_sample
  sample.translation = 1
  sample.rotation = 0.5
  sampleWriter.write(sample)

  Readline::readline("Press Enter to exit\n") do
  end
end

and run it with

ruby rockTutorial1.rb

As in the basics tutorial, the Ruby commands lead to a start of the task, i.e. calling the startHook() of the task. Afterwards we use a port writer to write a motion command to the ‘motion_command’ port.

In another console start the task inspector with

rock-display rock_tutorial_control

Right-klicking the output port pose gives you the option to use RigidBodyStateVisualization to show the rock’s state. It will be presented by coordinate axes showing position and orientation of the rock.

If you want to steer the rock using a joystick, progress to the next tutorial. If you don’t have a joystick, go steer using a graphical interface.