Project 5: Inverse Kinematics

Due at 11:59 PM on April 18

Updates

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

Update Thursday April 3 at 10:30 AM:
- Update to IKSolver::solve and IKSolver::solve_with_trajectory, includes a bool & in function signature for returning convergence state
- Update to tests, rather than checking if joint positions are equal to expected, the tests check if the returned configuration produces the expected end effector pose.
- Update to the JRDF file, removed simple_bot.json and replaced with RRP.json which is the Revolute-Revolute-Prismatic robot used in lecture on Wed. April 2.

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/c83anurgogy1djdqcj3aj/Project5.tar.gz?rlkey=fssurzsu5y0uy6xlox3gc30el&st=pxyysttv&dl=0" -O p5.tar.gz
tar -xf p5.tar.gz
rm p5.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

Understanding and implementing inverse kinematics using gradient descent.
Computing the geometric Jacobian for a serial chain robot.
Using the pseudoinverse of the Jacobian for iterative optimization.
Developing a service to solve inverse kinematics for a given robot description.

Overview

Please refer to the project 5 walkthrough on inverse kinematics and creating the Jacobian here. In this project, you are responsible for implementating an interface to compute the configuration of a robot that produces a given end effector pose. You will then create a RIX node that will provide this interface as a service to other nodes on the network.

Starter Code

The starter code for Project 5 is nearly the same as Project 4. The only difference is that there are no source files for the rixrdf library. Instead, the solution to Project 4 has been precompiled and is located at lib/librixrdf.a.

The following directories contain files that are dependencies for the IKSolver class and the iksrv node.

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/
- The rixutil, rixipc, rixcore, and rixrdf packages are precompiled as static libraries for CAEN. rixrdf is the precompiled solution code from Project 4.

These directories contain files with TODOs.

include/rix
- core/
  - Headers for the rixcore classes, such as Publisher, Subscriber, and Node.
- 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. iksrv/IKRequest, iksrv/IKResponse, 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 classes, such as Joint, Link, Tree, and FKSolver
- ik_solver.hpp
  - Header file for the IKSolver class that you will implement in this project.
src/
- ik_solver.cpp
  - Source file for the IKSolver class that you will implement in this project.
- iksrv.cpp
  - Executable for the iksrv node that you will implementi 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 IKSolver class.
- manual_tests/
  - Manual tests for the iksrv node.

5.1 IKSolver

The IKSolver class is responsible for computing the configuration of joints in a serial chain that produces a given end effector pose. To perform this operation, we will use gradient descent with the geometric Jacobian of the robot arm. Take a look at slides 94-100 in the inverse kinematics lecture.

Calculate the error between the end effector in its initial configuration to the goal pose of the end effector (this is done via vector subtraction). Use the FKSolver from Project 4 to find the end effector pose from the robot’s current configuration.
Compute the geometric Jacobian of the serial chain with the given end effector. Computation of the Jacobian need only to occur with respect to the joints along the chain from the endeffector joint to the robot base.
Compute the pseudoinverse of the Jacobian and use it to determine the step direction for the current iteration by multiplying it with the endpoint error.
Multiply the step direction by the step size and accumulate it with the solution configuration.
Repeat until the change in configuration is less than the specified tolerance (convergence) or the maximum number of iterations is reached.

TODOS

Private Member Variables and Methods
- Declare any private member variables or helper methods needed for the implementation of the inverse kinematics solver. For example:
  - A method to compute the Jacobian matrix for a given robot configuration.
  - A method to compute the pseudoinverse of the Jacobian.
  - A method to perform gradient descent for iterative optimization.
Implement solve Method
- Compute the Jacobian matrix for the given robot configuration.
- Calculate the pseudoinverse of the Jacobian.
- Use gradient descent to iteratively update the joint configuration to minimize the error between the current pose and the goal pose.
- Stop the iteration when the error is within the specified tolerance or the maximum number of iterations is reached.
- Return the final joint configuration as a rix::msg::sensor::JS object.
Implement solve_with_trajectory Method:
- Extend the solve method to compute a trajectory of joint configurations leading to the goal pose.
- Store intermediate joint configurations at each iteration of the gradient descent process.
- Return the trajectory as a vector of rix::msg::sensor::JS objects.

5.2 Inverse Kinematics Service

The iksrv RIX node will advertise a service to solve the inverse kinematics for a given robot description (the jrdf command line argument). The request type for the service should be iksrv/IKRequest and the response type should be iksrv/IKResponse. These types are displayed below.

namespace rix {
namespace msg {
namespace iksrv {

class IKRequest : public MessageBase {
  public:
    std::string end_effector;
    geometry::Transform goal;
    sensor::JS initial_guess;
    double step_scale;
    double tolerance;
    uint32_t max_iterations;
    bool debug;
};

class IKResponse : public MessageBase {
  public:
    sensor::JS result;
    bool converged;
    bool debug;
    std::vector<sensor::JS> steps;
};

} // namespace iksrv
} // namespace msg
} // namespace rix

When the service receives a request, it should use the IKSolver class to compute the configuration of the robot given the end effector name and the goal transform. IKRequest::goal is a trasform from the world frame to the end effector frame. The other fields can be passed as input the IKSolver::solve method. The service should respond with the IKResponse where IKResponse::result is the configuration of the robot that produces the given end effector transform. If the gradient descent calculation fails to converge within the specified number of iterations, set IKResponse::converged to false.

When IKRequest::debug is true, the service should fill the IKResponse::steps field with the intermediate joint configurations from each iteration of the gradient descent process.

Building

You can build the project with the following commands. You must build on CAEN. The libraries (librixcore.a, librixipc.a, librixutil.a, librixrdf.a) were compiled for CAEN. This project will not build on your local machine (unless you have WSL or Ubuntu, see here).

mkdir build
cd build
cmake ..
make

The unit test executable is test_ik_solver.

The inverse kinematics service executable is iksrv.

The manual test executale is test_iksrv.

Testing

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

There are also manual tests in test/manual_tests for iksrv. This test will make several calls to the service and test the responses.