Project 4: Forward Kinematics

Due at 10:30 AM on March 31

Updates

When updates to the project occur (bug fixes, new files, etc.) they will be placed here.

  1. Update Sunday March 16 at 2:30PM
    • Remove the extra executables (js_listener.cpp and tf_listener.cpp) from src/
    • Replace test/manual_tests/joint_test.cpp with this new version.
  2. Link to slides from Project 4 Walkthrough on March 17.
  3. Update the compare_jsp_test.cpp and compare_rsp_test.cpp so they search for the solution binary in the correct directory. Change test/sol_js_msgs.bin to test/bin/sol_js_msgs.bin and test/sol_tf_msgs.bin to test/bin/sol_tf_msgs.bin.
  4. Update Monday March 24 at 10:30 AM
    • Add #include <mutex> to include/rix/util/log.hpp and #include <array> to include/rix/msg/message_base.hpp.
    • Replace test/manual_tests/compare_rsp_test.cpp with this new version.
    • Add the maximum rate command line argument to rsp.cpp. The beginning of your main function for RSP should be the following.
CLParser parser("robot_state_publisher", "Publishes header messages");
parser.add_arg(CLParser::Arg("jrdf", "Path to JRDF file", '1'));
parser.add_opt(CLParser::Opt("ip", "i", "RIX Hub IP Address", "127.0.0.1", "", '1'));
parser.add_opt(CLParser::Opt("rate", "r", "Max publish rate in rate", "20", "", '1'));
parser.parse(argc, argv);

std::string jrdf_file = parser.get_arg("jrdf").front();
std::string hub_ip = parser.get_opt("ip").front();
double rate = std::stod(parser.get_opt("rate").front());

Getting the Starter Code

Navigate to the directory where you would like to store the code for this project. Run the following commands to install the project starter code or download it manually here.

wget "https://www.dropbox.com/scl/fi/etrwf6zy7rsdp63xbak2e/Project4.tar.gz?rlkey=w0eu5wsgfet4gwgf4kri23ar8&st=zp3uegca&dl=0" -O p4.tar.gz
tar -xf p4.tar.gz
rm p4.tar.gz

It is strongly recommended to use GitHub to track the changes made to your project. First, create a new private repository in GitHub. Then, navigate to the project directory and run the following commands.

git init
git commit -m "first commit"
git branch -M main
git remote add origin [LINK TO REPO]
git push -u origin main

This will establish a main branch on the remote repository and add the starter code as the first commit.

Learning Objectives

  1. Understand representing robot geometry in text-based formats
  2. Implement an interface for representing a robot manipulator as a kinematic chain
  3. Create an interface for solving the forward kinematics for a robot manipulator
  4. Create a program to publish synchronized joint states for a given robot description
  5. Create a program to publish synchronized transforms for a given robot description

Overview

This project will implement the rixrdf package. rixrdf contains interfaces for representing robot geometry in software and calculating the position of a robot’s links given its configuration (joint positions). Additionally, you will create the joint state publisher (JSP) and robot state publisher (RSP) applications as RIX nodes. These are useful tools in robot systems which enable their subscribers to calculate the pose of any link in the description relative to the world frame or other links. Below is a description of the starter code.

Starter Code

The following directories contain files that are dependencies for the rixrdf library and the JSP and RSP nodes.

  • eigen/
    • The Eigen linear algebra library. Eigen/Geometry provides classes such as Affine3d, Vector3d, and Quaterniond.
  • json/
    • The nlohmann/Json JSON parsing library. This is used by the Tree class to parse a robot description. You do not need to use this library in this project. You have already implemented the robot description parser in Lab 6.
  • lib/
    • rixutil, rixipc, and rixcore packages precompiled as static libraries for CAEN. rixcore is the precompiled solution code from Project 3. Your node implementations will link against this library.

These directories contain files with TODOs. rixrdf is in include/rix/rdf/ and src/rix/rdf.

  • include/rix
    • core/
      • Headers for the rixcore classes, such as Publisher, Subscriber, and Node. You will use these in your node implementations.
    • util/
      • Interfaces for parsing command line arguments and logging data to stdout.
    • ipc/
      • Interfaces for nterprocess communication. You do not need to use this directly! rixcore depends on this.
    • msg/
      • Message types relevent to this project. geometry/TF, sensor/JS, and their dependencies are important! Please look over the member variables for these message types. You do not need to serialize/deserialize any messages in this project.
    • rdf/
      • Headers for the rixrdf package that you will implement in this project.
  • src/
    • rix/rdf
      • Source files for the rixrdf package that you will implmement in this project.
    • Source files for the joint state publisher and robot state publisher that you will implement in this project.

These directories contain files related to testing or running your programs.

  • robots/
    • Robot description files.
  • bin/
    • rixhub executable precompiled for CAEN.
  • googletest/
    • GoogleTest testing framework.
  • test/
    • unit_tests/
      • Public tests for the Joint, Tree, and FKSolver classes.
    • manual_tests/
      • Manual tests for the JSP and RSP nodes.
    • bin
      • Binary files with data to compare output for JSP and RSP nodes.

4.1 rixrdf

Implement the classes for representing a robot as a kinematic chain.

TODOs

  1. src/rix/rdf/joint.cpp
    • Implement the Joint::get_transform function. This function should return the transform based on the current joint position and type.
  2. src/rix/rdf/tree.cpp
    • Implmement the Tree::TF function. This should return a TF message based on the current configuration of the robot. This message should include the transform for every joint from its parent to its child link. The Header::stamp field of every TransformStamped should include the current time. Header::frame_id should be set to the name of the parent link. TransformStamped::child should be set to the name of the child link.
    • Implmement the Tree::JS function. This should return a JS message containing the joint positions of the robot. The JS::stamp field should include the current time. The JS::joint_states field should contain the name and position of every joint in the robot description.
    • Implement the Tree::set_state function. For each joint in the input JS message, set the state of the corresponding joint within the tree. If the joint does not exist, ignore it and optionally log a warning.
  3. src/rix/rdf/fk_solver.cpp and include/rix/rdf/fk_solver.hpp.
    • Implement the FKSolver::solve functions. There are four overloads of solve. These functions require recursion! You can define private recursive helper functions in the FKSolver class in include/rix/rdf/fk_solver.hpp.
      1. Eigen::Affine3d solve(const std::string &link_name) returns the transform from the world frame to the link_name frame based on the current configuration of the tree passed into the constructor.
      2. Eigen::Affine3d solve(const std::string &link_name, const std::string &reference_link) returns the transform from the reference_link frame to the link_name frame based on the current configuration of the tree passed into the constructor.
      3. static Eigen::Affine3d solve(const TF &tf, const std::string &link_name) returns the transform from the world frame to the link_name frame based on the TF message passed into the function.
      4. static Eigen::Affine3d solve(const TF &tf, const std::string &link_name, const std::string &reference_link) returns the transform from the reference_link frame to the link_name frame based on the TF message passed into the function.
    • Implement the FKSolver::global_tf functions. There are two overloads of global_tf. These functions require recursion! You can define private recursive helper functions in the FKSolver class in include/rix/rdf/fk_solver.hpp.
      1. TF global_tf() const returns a TF message that contains transfroms from the world frame to the frame of each link based on the current configuration of the tree passed into the constructor.
      2. static TF global_tf(const TF &tf) const returns a TF message that contains transforms from the world frame to the frame of each link specified in the input TF message.

4.2 Joint State Publisher

Create a program to publish synchronized joint states for a given robot description specified by the command line argument jrdf. This program will subscribe to the topics specified by the command line argument topics and publish on the joint_states topic at a fixed rate specified by the command line option rate, which has a default value of 50 Hz. The message type of the topics JSP subscribes to and the topic it publishes to is rix::msg::sensor::JS.

Each time a message is received on one of the subscribed topics, the joint positions from the messages should be saved in a container. This will occur in a separate thread from within a callback function specified to the subscriber. Ensure that you are protecting this container with the proper synchronization mechanism!

In the main thread of the program, use the rix::util::Rate class to publish all of the saved joint states at the specified frequency. Ensure that you set the stamp field of the JS message to the current time when you publish. You will need to read the joint state data from the container being written to by the callback functions. Ensure that you are protecting this container with the proper synchronization mechanism!

4.3 Robot State Publisher

Create a program to publish synchronized transforms for a given robot description specified by the command line argument jrdf. This program will subscribe to the joint_states topic (published to by JSP) and will publish at a maximum rate specified by the command line option rate. When a JS message is received, this program should update the configuration of the robot, generate a TF message, and publish it on the tf topic.

The Robot State Publisher should implmement a rate limiting algorithm on the input messages. RSP should always publish at a frequency less than or equal to the maximum frequency. To ensure this, only calculate the TF and publish if the timestamp of the new message is at least a publish period greater than the least recently published joint of the message.

Building

You can build the project with the following commands. You must build on CAEN. The libraries (librixcore.a, librixipc.a, and librixutil.a) were compiled for CAEN. This project will not build on your local machine.

mkdir build
cd build
cmake ..
make

The unit test executables are joint_test, tree_test, and fk_solver_test.

The joint state publisher and robot state publisher executables are jsp and rsp.

The manual test executales are jsp_simulator, joint_publisher, js_listener, tf_listener, js_recorder, tf_recorder, compare_jsp_test, and compare_rsp_test.

Testing

We have heard your feedback on providing public tests within the projects, so we have included some unit tests for you to test your rixrdf implementation. These tests are in test/unit_tests.

There are also manual tests in test/manual_tests for JSP and RSP. The two tests are named compare_jsp_test.cpp and compare_rsp_test.cpp. The intention with these tests was to record JS and TF messages published by the JSP and RSP nodes when run in parallel with nodes that simulate joint positions. This recorded data can then be compared against the data recorded from the solution implementation. Instructions for replicating these tests are in the compare_jsp_test.cpp and compare_rsp_test.cpp files.