Class Meeting 9: Forward & Inverse Kinematics


Today's Class Meeting


Class Exercise #1: Computing Forward Kinematics for 3DOF Robot Manipulator in 3 Dimensions

For this exercise, work independently to compute \([x, y, z]\) of the robot's end effector in terms of the lengths of the robot's two linkages \((l_1, l_2)\) and the angles of rotation of the two revolute joints \( (q_1, q_2) \). If it's useful to you, feel free to reference the slides we just went over in class.

forward kinematics 3D 2DOF manipulator

Once you've finished computing \([x, y, z]\), please wait for others to finish. We'll go over the solution together as a class.


Class Exercise #2: Using the OpenManipulatorX GUI

The goal of this exercise is to start getting to know the OpenManipulatorX arm that we'll be using on our Turtlebot4. you will (1) learn the basics of turtlebot's arms though GUIs, (2) learn how to manipulate the arm through code. We'll be using the arm in simulation for this exercise, you'll get to use it on the physical Turtlebot4 for the first time during tomorrow's lab.

Setup

Before getting started on this exercise, you'll need to download and build the OpenManipulator ROS2 packages in your intro_robo_ws: 

$ cd ~/intro_robo_ws/src
$ git clone -b humble https://github.com/ROBOTIS-GIT/open_manipulator.git
$ git clone https://github.com/Intro-Robotics-UChicago-Fall-2025/omx_cpp_interface.git
$ cd ~/intro_robo_ws
$ colcon build --symlink-install
$ source install/setup.bash

Working with the OpenManipulatorX GUI

Let's now startup an OpenManipulatorX arm in simulation and control it with the OpenManipulatorX GUI.

The GUI provides the following information:

Let's make sure your simulation robot can move its arm. Press Time Start and then click on Home Pose. You should see your robot arm move to a new position in the simulation. Pay close attention to how the new pose changes the joint and XYZ values.

Manual movement of the arm is possible through the options in the bottom right features of the GUI. There are two options available for this purpose,

The window with an s next to it is for selecting how fast you want the the transformation to occur. Once you are done with your manual selection, send your desired positions to the robot and watch the arm move.

The following gif shows a demonstration of using the OpenManipulatorX GUI to control the arm in simulation.

OpenManipulatorX GUI Demo.

Class Exercise #2 Homework Deliverables

For your first set of homework deliverables, submit screenshots that include both the GUI and simulated OpenManipulatorX arm in Gazebo for the following:


Class Exercise #3: Writing Code to Control the OpenManipulatorX Arm

For this exercise, you will construct a new ROS2 package, from which you will send ROS2 messages that will control the OpenManipulatorX arm in simulation.

Running the Code

To run your code, follow these steps:


omx_cpp_interface

The omx_cpp_interface package provides a simple C++ interface to control the OpenManipulatorX arm using the MoveIt package. I (Sarah) wrote this package because MoveIt 2, a powerful motion planning framework, can only be used in ROS2 in C++ (and not Python). You do not need to implement anything in this package, but you should understand how to use it to control the arm through code.

The omx_cpp_interface package subscribes to the following ROS2 topics that you can publish to from your Python code (assuming that you're connected to Turtlebot number 11):

ROS2 Topic Message Type Description
/tb11/target_pose geometry_msgs/Pose You can publish a target end-effector pose message to this topic and the omx_cpp_interface will use it to set the target pose of the end effector.
/tb11/target_joint_angles omx_cpp_interface/ArmJointAngles You can publish desired joint angles of the arm and the omx_cpp_interface will use these angles to move to the specified configuration.
/tb11/target_gripper_position omx_cpp_interface/ArmGripperPosition You can publish the desired gripper locations of the left and right gripper components and the omx_cpp_interface will use these locations to move the gripper.

Example Code Snippet for Gripper Control

The following code is an example of a ROS2 Python node that publishes to the /tbXX/target_gripper_position topic to set the gripper position of the OpenManipulatorX arm. You can use this code as a starting point for your own code to control the arm. 

import os
import time
import rclpy
from rclpy.node import Node
from omx_cpp_interface.msg import ArmGripperPosition


class GripperPublisher(Node):
    def __init__(self):
        # initialize the node 
        super().__init__('gripper_publisher')

        # get the ROS_DOMAIN_ID aka robot number
        ros_domain_id = os.getenv("ROS_DOMAIN_ID", "0")
        try:
            if int(ros_domain_id) < 10:
                ros_domain_id = "0" + str(int(ros_domain_id))
            else:
                ros_domain_id = str(int(ros_domain_id))
        except Exception:
            ros_domain_id = "00"
        self.get_logger().info(f'ROS_DOMAIN_ID: {ros_domain_id}')

        # create publisher to the target_gripper_position topic
        topic = f'/tb{ros_domain_id}/target_gripper_position'
        self.pub = self.create_publisher(ArmGripperPosition, topic, 10)

    def publish_once(self):
        # publish a message to set the gripper position
        msg = ArmGripperPosition()
        msg.left_gripper = 0.005
        msg.right_gripper = 0.005
        self.pub.publish(msg)
        self.get_logger().info(f'Published ArmGripperPosition: left={msg.left_gripper} right={msg.right_gripper}')


def main(args=None):
    # initialize the node
    rclpy.init(args=args)
    node = GripperPublisher()

    # wait a couple seconds to ensure connections are established
    time.sleep(2)

    # publish the gripper position
    try:
        node.publish_once()
        # spin once to allow the outgoing message to be delivered
        rclpy.spin_once(node, timeout_sec=1.0)
    except KeyboardInterrupt:
        pass

    # shut down the node
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

Class Exercise #3 Homework Deliverables

Your goal in this exercise is to create a new ROS2 Python package that contains a node that publishes to the topics outlined in the table above to control both the position of the arm and the gripper.

Using your code, submit the following screenshots that contain both your terminal with your code running and the Gazebo simulation window showing the OpenManipulatorX arm in the desired position:


Class Meeting 9 Homework

For your Class Meeting 9 Homework, submit the following via Canvas/Gradescope by 2:00pm on Wednesday, October 29th:


Acknowledgments

I want to thank Woolfrey for their helpful Youtube videos on forward and inverse kinematics for helping to inform the content for today's class.


© Sarah Sebo