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.