Moveit2 - 4. robot model and robot state

Robot model and robot state

In this tutorial , Will guide you to understand MoveIt2 Using kinematics C++ API.

RobotModel and RobotState class

RobotModel and RobotState Class is MoveIt2 The core of the class , Allows users to access robot kinematics .

RobotModel Class contains all the relationships between links and joints , Ranging from URDF Loaded joint limits and other attributes .RobotModel Classes also divide the links and joints of robots into SRDF Multiple planning groups defined in . You can find information about URDF and SRDF A separate tutorial ：URDF and SRDF course

RobotState Class contains information about the robot at a certain point in time , Store joint position vectors and optional speeds and accelerations . This information can be used to obtain kinematic information about the robot , This information depends on the current state of the robot , For example, the Jacobian matrix of the end effector .

RobotState The class also contains functions for determining the position of the end effector （ Cartesian pose ） Auxiliary functions for setting arm position and calculating Cartesian trajectory .

In this tutorial example , It will gradually guide users to understand these classes in Panda Processes used on robots .

Run code

All the code in this tutorial can be viewed as MoveIt2 Install one of the components moveit2_tutorials Compile and run in the software package .

Use ros2 launch The order comes directly from moveit2_tutorials The startup file in the software package to run the code ：

ros2 launch moveit2_tutorials robot_model_and_robot_state_tutorial.launch.py

Expected output

The expected output will take the following form . Because the random joint value is used here , So the numbers will be different ：

... [robot_model_and_state_tutorial]: Model frame: world
... [robot_model_and_state_tutorial]: Joint panda_joint1: 0.000000
... [robot_model_and_state_tutorial]: Joint panda_joint2: 0.000000
... [robot_model_and_state_tutorial]: Joint panda_joint3: 0.000000
... [robot_model_and_state_tutorial]: Joint panda_joint4: 0.000000
... [robot_model_and_state_tutorial]: Joint panda_joint5: 0.000000
... [robot_model_and_state_tutorial]: Joint panda_joint6: 0.000000
... [robot_model_and_state_tutorial]: Joint panda_joint7: 0.000000
... [robot_model_and_state_tutorial]: Current state is not valid
... [robot_model_and_state_tutorial]: Current state is valid
... [robot_model_and_state_tutorial]: Translation:
-0.368232
0.645742
0.752193

... [robot_model_and_state_tutorial]: Rotation:
 0.362374 -0.925408  -0.11093
 0.911735  0.327259  0.248275
 -0.193453 -0.191108  0.962317

... [robot_model_and_state_tutorial]: Joint panda_joint1: 2.263889
... [robot_model_and_state_tutorial]: Joint panda_joint2: 1.004608
... [robot_model_and_state_tutorial]: Joint panda_joint3: -1.125652
... [robot_model_and_state_tutorial]: Joint panda_joint4: -0.278822
... [robot_model_and_state_tutorial]: Joint panda_joint5: -2.150242
... [robot_model_and_state_tutorial]: Joint panda_joint6: 2.274891
... [robot_model_and_state_tutorial]: Joint panda_joint7: -0.774846
... [robot_model_and_state_tutorial]: Jacobian:
  -0.645742  -0.26783  -0.0742358  -0.315413  0.0224927  -0.031807  -2.77556e-17
  -0.368232  0.322474  0.0285092  -0.364197  0.00993438  0.072356  2.77556e-17
    0  -0.732023  -0.109128  0.218716  2.9777e-05  -0.11378  -1.04083e-17
    0  -0.769274  -0.539217  0.640569  -0.36792  -0.91475  -0.11093
    0  -0.638919  0.64923  -0.0973283  0.831769  -0.40402  0.248275
    1  4.89664e-12  0.536419  0.761708  0.415688  -0.00121099  0.962317

notes ： If your output has different ROS Console format , Please don't worry .

Complete code

start-up

Use RobotModel Class is very simple . Usually , You will find that most of the more advanced components will return a point RobotModel Class shared pointer . If possible , You should always use this pointer . In this example , Such a shared pointer will be used to start , And only the basic API. You can view the actual code of these classes API, For more details on how to use the more functionality provided by these classes .

We will first instantiate a RobotModelLoader object , The object will be in ROS Find the robot description on the parameter server and build a RobotModel For our use .

robot_model_loader::RobotModelLoader robot_model_loader(node);
const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel();
RCLCPP_INFO(LOGGER, "Model frame: %s", kinematic_model->getModelFrame().c_str());

Use RobotModel, We can build a RobotState To maintain the configuration of the robot . We set all joints in this state to the default value . Then you can get one JointModelGroup, It represents a robot model of a specific planning group , for example Panda Robotic “panda_arm” Planning Group .

moveit::core::RobotStatePtr kinematic_state(new moveit::core::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
const moveit::core::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("panda_arm");

const std::vector<std::string>& joint_names = joint_model_group->getVariableNames();

Get joint values

We can retrieve stored in Panda The current set of joint values in the arm planning group state ：

std::vector<double> joint_values;
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
for (std::size_t i = 0; i < joint_names.size(); ++i)
{
    
  RCLCPP_INFO(LOGGER, "Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
}

Joint limitation

function setJointGroupPositions() Joint restrictions are not enforced by itself , But the call enforceBounds() Function will execute joint restriction .

/* Set one joint in the Panda arm outside its joint limit */
joint_values[0] = 5.57;
kinematic_state->setJointGroupPositions(joint_model_group, joint_values);

/* Check whether any joint is outside its joint limits */
RCLCPP_INFO_STREAM(LOGGER, "Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));

/* Enforce the joint limits for this state and check again*/
kinematic_state->enforceBounds();
RCLCPP_INFO_STREAM(LOGGER, "Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));

Forward kinematics

Now? , We can calculate the forward kinematics of a set of random joint values （FK）. Please note that , Here we want to find robots “panda_arm” The farthest link in the planning group “panda_link8” The position of .

kinematic_state->setToRandomPositions(joint_model_group);
const Eigen::Isometry3d& end_effector_state = kinematic_state->getGlobalLinkTransform("panda_link8");

/* Print end-effector pose. Remember that this is in the model frame */
RCLCPP_INFO_STREAM(LOGGER, "Translation: \n" << end_effector_state.translation() << "\n");
RCLCPP_INFO_STREAM(LOGGER, "Rotation: \n" << end_effector_state.rotation() << "\n");

Inverse kinematics

We can do it now for Panda Robot solves inverse kinematics （IK）. Demand solution IK, We need the following information ：

Desired posture of the end actuator （ By default “panda_arm” The last link in the chain ）： That is, calculated in the previous step end_effector_state.

Timeout time ：0.1s

double timeout = 0.1;
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, timeout);

Now? , We can print out this IK Solution results （ If it is solved ）：

if (found_ik)
{
    
  kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
  for (std::size_t i = 0; i < joint_names.size(); ++i)
  {
    
    RCLCPP_INFO(LOGGER, "Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
  }
}
else
{
    
 RCLCPP_INFO(LOGGER, "Did not find IK solution");
}

Get Jacobian matrix

We can also from RobotState Get Jacobian matrix ：

Eigen::Vector3d reference_point_position(0.0, 0.0, 0.0);
Eigen::MatrixXd jacobian;
kinematic_state->getJacobian(joint_model_group,
                         kinematic_state->getLinkModel(joint_model_group->getLinkModelNames().back()),
                         reference_point_position, jacobian);
RCLCPP_INFO_STREAM(LOGGER, "Jacobian: \n" << jacobian << "\n");

launch file

To run this code , Need a startup file , The startup file should complete the following two things ：

take Panda Robotic URDF and SRDF Load to parameter server ;

take MoveIt Set assistant generated kinematics_solver Configure push to ROS The parameter server instantiates the node namespace of each class in this tutorial .

import os
import yaml
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
from ament_index_python.packages import get_package_share_directory
import xacro


def load_file(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, "r") as file:
            return file.read()
    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
        return None


def load_yaml(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, "r") as file:
            return yaml.safe_load(file)
    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
        return None


def generate_launch_description():
    # planning_context
    robot_description_config = load_file(
        "moveit_resources_panda_description", "urdf/panda.urdf"
    )

    robot_description_semantic_config = load_file(
        "moveit_resources_panda_moveit_config", "config/panda.srdf"
    )

    kinematics_yaml = load_yaml(
        "moveit_resources_panda_moveit_config", "config/kinematics.yaml"
    )

    tutorial_node = Node(
        package="moveit2_tutorials",
        executable="robot_model_and_robot_state_tutorial",
        output="screen",
        parameters=[
            {
    "robot_description": robot_description_config},
            {
    "robot_description_semantic": robot_description_semantic_config},
            kinematics_yaml,
        ],
    )

    return LaunchDescription([tutorial_node])