Welcome to DOFBOT-Ultra repository

Inverse Kinematics

Please ensure that the web control program backend process is not running in the background. To close it, refer to the [Get Started Quickly - 2. Access the Terminal] section in [Disable the Auto-Start Web Control Backend Process]. After closing, please restart DofbotUltra!!!

Before starting this function, you need to close the web control program backend process.

1. Function Description

After the node program starts, it provides a /dofbot_kinematics service that offers both fk (forward kinematics) and ik (inverse kinematics) services. You can choose which service to call by specifying kin_name. The fk service takes the current angle values of the 6 servos (joint values) as input and outputs the current end-effector pose of the robotic arm. The pose includes xyz coordinates and Euler angles (roll, pitch, yaw). The ik service takes the end-effector pose (xyz coordinates and Euler angles) as input and outputs the 6 servo angle values (joint values). In subsequent 3D space object grasping operations, we need to first read the current end-effector pose, then identify the object's center and calculate a new pose coordinate through coordinate transformation, and finally call ik to compute each servo value.

2. Startup and Operation

2.1. Startup

Enter in the terminal:

xxxxxxxxxx
# Start the inverse kinematics program
ros2 run dofbot_ultra_info kinematics_dofbotultra
# Start the ROS control program for the robotic arm
ros2 run dofbot_ultra_driver arm_driver

After the inverse kinematics node program starts, it provides a service. Use the following command to check:

xxxxxxxxxx
ros2 node info /kinematics_dofbotultra

In the service list, there is /dofbot_kinematics, which is the inverse kinematics service. Use the following command to check its related information:

xxxxxxxxxx
ros2 service type /dofbot_kinematics

xxxxxxxxxx
#### Output Information #######
root@raspberrypi:/home/yahboom/dofbot_ultra# ros2 node info /kinematics_dofbotultra
/kinematics_dofbotultra
  Subscribers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
  Publishers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /rosout: rcl_interfaces/msg/Log
  Service Servers:
    /dofbot_kinematics: dofbot_ultra_interface/srv/Kinematics
    /kinematics_dofbotultra/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /kinematics_dofbotultra/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /kinematics_dofbotultra/get_parameters: rcl_interfaces/srv/GetParameters
    /kinematics_dofbotultra/list_parameters: rcl_interfaces/srv/ListParameters
    /kinematics_dofbotultra/set_parameters: rcl_interfaces/srv/SetParameters
    /kinematics_dofbotultra/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
  Service Clients:

  Action Servers:

  Action Clients:

root@raspberrypi:/home/yahboom/dofbot_ultra# ros2 service type /dofbot_kinematics
dofbot_ultra_interface/srv/Kinematics
root@raspberrypi:/home/yahboom/dofbot_ultra#

The /dofbot_kinematics service type is the custom dofbot_ultra_interface/srv/Kinematics. The specific parameters are in the Args section below. We can use the following command to view what content is included in the dofbot_ultra_interface/srv/Kinematics data type:

xxxxxxxxxx
ros2 interface show dofbot_ultra_interface/srv/Kinematics

The --- divides the content into two parts: the upper part is the request (input parameters), and the lower part is the response (output return values). The request part has an additional kin_name field, which distinguishes whether we are calling IK or FK. If you want to call the IK service, assign IK to kin_name. Conversely, assign FK to kin_name to call the FK service.

Request parameters:

• tar_x: The x value of the target position, unit is m

• tar_y: The y value of the target position, unit is m

• tar_z: The z value of the target position, unit is m

• Roll: The Roll value of the target pose, unit is radians

• Pitch: The Pitch value of the target pose, unit is radians

• Yaw: The Yaw value of the target pose, unit is radians

The above 6 parameters must be assigned when calling IK.

• cur_joint1: Current angle value of servo #1, unit is degrees

• cur_joint2: Current angle value of servo #2, unit is degrees

• cur_joint3: Current angle value of servo #3, unit is degrees

• cur_joint4: Current angle value of servo #4, unit is degrees

• cur_joint5: Current angle value of servo #5, unit is degrees

• cur_joint6: Current angle value of servo #6, unit is degrees

The above 6 parameters must be assigned when calling FK.

• kin_name: The name of the service to call, either ik or fk

Response parameters:

• joint1: Angle value of servo #1, unit is degrees

• joint2: Angle value of servo #2, unit is degrees

• joint3: Angle value of servo #3, unit is degrees

• joint4: Angle value of servo #4, unit is degrees

• joint5: Angle value of servo #5, unit is degrees

• joint6: Angle value of servo #6, unit is degrees

The above 6 values are the servo angle values returned after calling the IK service.

• x: The calculated x value of the end-effector position, unit is m

• y: The calculated y value of the end-effector position, unit is m

• z: The calculated z value of the end-effector position, unit is m

• Roll: The calculated Roll value of the end-effector position, unit is radians

• Pitch: The calculated Pitch value of the end-effector position, unit is radians

• Yaw: The calculated Yaw value of the end-effector position, unit is radians

The above 6 parameters are the end-effector position and pose returned after calling the FK service.

2.2. Operation

2.2.1. FK (Forward Kinematics)

First, let's move the robotic arm to a certain state. Then, based on this state, we read the end-effector position and pose and compare them with actual measurements to check accuracy. From the previous lesson "ROS Control Robotic Arm," we can use a topic to publish a robotic arm pose. For example, publish a pose of [90, 120, 30, 30, 90, 90]. Enter in the terminal:

xxxxxxxxxx
ros2 topic pub --once /TargetAngle dofbot_ultra_interface/ArmJoint "{id: 0, run_time: 1000, angle: 0.0, joints: [90, 120, 30, 30, 90, 90]}"

Press Enter to publish the message, and the robotic arm moves to the specified pose. Then, we call the FK service and input the just-set [90, 120, 30, 30, 90, 90] as the six servo current values, i.e., cur_joint1 through cur_joint6. Also, set kin_name to fk. Enter in the terminal:

xxxxxxxxxx
ros2 service call /dofbot_kinematics dofbot_ultra_interface/srv/Kinematics "
{
  tar_x: 0.0,
  tar_y: 0.0,
  tar_z: 0.0,
  roll: 0.0,
  pitch: 0.0,
  yaw: 0.0,
  cur_joint1: 90.0,
  cur_joint2: 120.0,
  cur_joint3: 30.0,
  cur_joint4: 30.0,
  cur_joint5: 90.0,
  cur_joint6: 90.0,
  kin_name: 'fk'
}
"

Press Enter, and the response will be printed in the terminal, as shown below:

From the printed response, we can see that the end-effector position xyz and the Euler angles (Roll, Pitch, Yaw) representing the end-effector pose are: x=-0.0006, y=0.1684148999974, z=0.268150411789, Roll=1.21669278515e-08, Pitch=-0.0, Yaw=0.0. From these data, we can see that the end-effector is 0.0006 meters to the right of the base coordinate system, 0.168414899974m in front, and 0.26810411789m high. The pose can be considered unchanged, as all Euler angles are equal to zero or very close to zero. We can use a ruler to roughly measure the end-effector position relative to the base coordinate system and check if it matches the calculated xyz values.

2.2.2. IK (Inverse Kinematics)

First, let's send a message to make the robotic arm extend upward. Enter in the terminal:

xxxxxxxxxx
ros2 topic pub /TargetAngle dofbot_ultra_interface/ArmJoint "{id: 0, run_time: 1000, angle: 0.0, joints: [90, 90, 90, 90, 90, 90]}"

Then, we call the IK service. Given the end-effector position and pose parameters, we calculate what each servo value of the robotic arm should be to achieve that position and pose. Taking the xyz and Euler angles from the previous FK calculation as the target position and pose, enter in the terminal:

xxxxxxxxxx
ros2 service call /dofbot_kinematics dofbot_ultra_interface/srv/Kinematics "
{
  tar_x: -0.0006,
  tar_y: 0.1684148999974,
  tar_z: 0.268150411789,
  roll: 1.21669278515e-08,
  pitch: 0.0,
  yaw: 0.0,
  cur_joint1: 0.0,
  cur_joint2: 0.0,
  cur_joint3: 0.0,
  cur_joint4: 0.0,
  cur_joint5: 0.0,
  cur_joint6: 0.0,
  kin_name: 'ik'
}
"

Press Enter, and the response will be printed in the terminal, as shown below:

From the printed response, the calculated joint1-joint6 correspond to the 6 servo values. After rounding each joint, they are [90, 120, 30, 30, 90, 0]. We try to publish the calculated joint1-joint6 to the robotic arm control node via topic to move it to the specified pose. Enter in the terminal:

xxxxxxxxxx
ros2 topic pub /TargetAngle dofbot_ultra_interface/ArmJoint "id: 0
run_time: 1000
angle: 0.0
joints: [90,120,30,30,90,0]"

Press Enter to move the robotic arm to the specified pose. Then, we use a ruler to measure whether the distance from the end-effector to the base coordinate system matches the input xyz values. An error within 1cm is acceptable. This is because each servo of the robotic arm cannot be guaranteed to be completely identical at the factory, and there are slight differences.

3. Program Flowchart

4. Core Code Analysis

Code paths:

xxxxxxxxxx
Raspberry Pi: /home/yahboom/dofbot_ultra_ws/src/dofbot_ultra_info/src/kinematics_dofbotultra.cpp
RDK: /home/sunrise/dofbot_ultra_ws/src/core/dofbot_ultra_info/src/kinematics_dofbotultra.cpp

Import related header files:

xxxxxxxxxx
#include <iostream>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include <kdl/chain.hpp>
#include <kdl/tree.hpp>
#include <kdl/segment.hpp>
#include <kdl/frames_io.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include "dofbot_ultra_interface/srv/Kinematics.hpp"
#include "dofbot_ultra_info/Kinematics_dofbotultra.h"

Define some constants:

xxxxxxxxxx
// Radian to degree
const float RA2DE = 180.0f / M_PI;
// Degree to radian
const float DE2RA = M_PI / 180.0f;
// URDF model. During inverse kinematics calculation, the URDF model is used as an input parameter.
// The 3D camera version description model filename in the current repository is:
const char *urdf_file = "/home/yahboom/dofbot_ultra_ws/src/core/dofbot_ultra_description/urdf/DOFBOT-Pro-3DCamera-URDF-V2.urdf";
//RDK:const char *urdf_file = "/home/sunrise/dofbot_ultra_ws/src/core/dofbot_ultra_description/urdf/DOFBOT-Pro-3DCamera-URDF-V2.urdf";

Main function:

xxxxxxxxxx
auto server_node = rclcpp::Node::make_shared("Kinematics_dofbot"); // Create service
auto service = server_node->create_service<dofbot_ultra_interface::srv::Kinematics>(
        "dofbot_kinematics", srvicecallback);

Service callback function srvicecallback:

FK service part:

xxxxxxxxxx
if (request->kin_name == "fk") {
        double joints[]{request->cur_joint1, request->cur_joint2, request->cur_joint3, request->cur_joint4,
                        request->cur_joint5};
        // Define target joint angle container
        vector<double> initjoints;
        // Define pose container
        vector<double> initpos;
        // Target joint angle unit conversion, from degrees to radians
        for (int i = 0; i < 5; ++i) initjoints.push_back((joints[i] - 90) * DE2RA);
        // Call forward kinematics function to get current pose
        dofbot_ultra.dofbot_getFK(urdf_file, initjoints, initpos);
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "fk sent***");
        response->x = initpos.at(0);
        response->y = initpos.at(1);
        response->z = initpos.at(2);
        response->roll = initpos.at(3);
        response->pitch = initpos.at(4);
        response->yaw = initpos.at(5);
    }

IK service part:

xxxxxxxxxx
// According to kin_name, the IK service is called
if (request->kin_name == "ik") {

        // Gripper length
        //double tool_param = 0.12;
        // Grasping pose
        double Roll = request->roll ;
        double Pitch = request->pitch;
        double Yaw = request->yaw ;
        double x=request->tar_x;
        double y=request->tar_y;
        double z=request->tar_z;
        // End-effector position (unit: m)
        double xyz[]{x, y, z};
        cout << x << y << z << endl;
        // End-effector orientation (unit: radians)
        //double rpy[]{Roll * DE2RA, Pitch * DE2RA, Yaw * DE2RA};
        double rpy[]{Roll , Pitch, Yaw };
        // Create output angle container
        vector<double> outjoints;
        // Create end-effector position container
        vector<double> targetXYZ;
        // Create end-effector orientation container
        vector<double> targetRPY;
        for (int k = 0; k < 3; ++k) targetXYZ.push_back(xyz[k]);
        for (int l = 0; l < 3; ++l) targetRPY.push_back(rpy[l]);
        // Inverse kinematics to calculate joint angles for reaching the target point
        dofbot_ultra.dofbot_getIK(urdf_file, targetXYZ, targetRPY, outjoints);
        // Print inverse kinematics results
        for (int i = 0; i < 5; i++) cout << (outjoints.at(i) * RA2DE) + 90 << ",";
        cout << endl;
        a++;
        response->joint1 = (outjoints.at(0) * RA2DE) + 90;
        response->joint2 = (outjoints.at(1) * RA2DE) + 90;
        response->joint3 = (outjoints.at(2) * RA2DE) + 90;
        response->joint4 = (outjoints.at(3) * RA2DE) + 90;
        response->joint5 = (outjoints.at(4) * RA2DE) + 90;

        // redis.set("joint1", std::to_string(response->joint1));
        cout<<"-----------------"<<endl;



    }