Robot model and robot state

Posted by reto on Sun, 20 Feb 2022 19:34:19 +0100

RobotModel and RobotState classes

The RobotModel and RobotState classes are the core classes for accessing robot kinematics.

The RobotModel class contains the relationships between all links and joints, including the joint limit attributes they load from URDF. The RobotModel also divides the robot's links and joints into planning groups defined in SRDF.
The RobotState contains information about the robot at a certain point in time, stores the vector of joint position and optional speed and acceleration. This information can be used to obtain kinematic information about the robot, which depends on its current state, such as the Jacobian determinant of the end effector.

The RobotState also contains auxiliary functions for setting the arm position and calculating the Cartesian trajectory based on the end effector position (Cartesian pose).

1, Routine

Roslaunch launches the file directly from moveit_tutorials run code:

roslaunch moveit_tutorials robot_model_and_robot_state_tutorial.launch

output

ros.moveit_tutorials: Model frame: /panda_link0
ros.moveit_tutorials: Joint panda_joint1: 0.000000
ros.moveit_tutorials: Joint panda_joint2: 0.000000
ros.moveit_tutorials: Joint panda_joint3: 0.000000
ros.moveit_tutorials: Joint panda_joint4: 0.000000
ros.moveit_tutorials: Joint panda_joint5: 0.000000
ros.moveit_tutorials: Joint panda_joint6: 0.000000
ros.moveit_tutorials: Joint panda_joint7: 0.000000
ros.moveit_tutorials: Current state is not valid
ros.moveit_tutorials: Current state is valid
ros.moveit_tutorials: Translation:
-0.541498
-0.592805
0.400443

ros.moveit_tutorials: Rotation:
-0.395039 0.600666 -0.695086
0.299981 -0.630807 -0.715607
-0.868306 -0.491205 0.0690048

ros.moveit_tutorials: Joint panda_joint1: -2.407308
ros.moveit_tutorials: Joint panda_joint2: 1.555370
ros.moveit_tutorials: Joint panda_joint3: -2.102171
ros.moveit_tutorials: Joint panda_joint4: -0.011156
ros.moveit_tutorials: Joint panda_joint5: 1.100545
ros.moveit_tutorials: Joint panda_joint6: 3.230793
ros.moveit_tutorials: Joint panda_joint7: -2.651568
ros.moveit_tutorials: Jacobian:
0.592805 -0.0500638 -0.036041 0.366761 -0.0334361 0.128712 -4.33681e-18
-0.541498 -0.0451907 0.0417049 -0.231187 0.0403683 0.00288573 3.46945e-18
0 -0.799172 0.0772022 -0.247151 0.0818336 0.0511662 0
0 0.670056 -0.742222 0.349402 -0.748556 -0.344057 -0.695086
0 -0.74231 -0.669976 -0.367232 -0.662737 0.415389 -0.715607
1 4.89669e-12 0.0154256 0.862009 0.021077 0.842067 0.0690048

2, Code

1. We will first instantiate a RobotModelLoader object, which will find the robot description on the ROS parameter server and construct a RobotModel for us to use.

The code is as follows (example):

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

Using the RobotModel, we can construct a RobotState to maintain the configuration of the robot. We set all joints in the state to their default values. Then we can get a jointmodel group, which represents the robot model of a specific group, such as "panda_arm" of Panda robot.

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();

Retrieves the current joint value set stored in the Panda arm 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)
{
  ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
}

2. Joint restrictions

etJointGroupPositions() does not enforce union restrictions by itself, but the call to enforceBounds() does

/* 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 */
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));

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

3. Forward kinematics

Now you can calculate the forward kinematics of a set of random joint values. Please note that we want to find the pose of "panda_link8", which is the farthest link in the robot "panda_arm" group.

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 */
ROS_INFO_STREAM("Translation: \n" << end_effector_state.translation() << "\n");
ROS_INFO_STREAM("Rotation: \n" << end_effector_state.rotation() << "\n");

4. Inverse kinematics

Now you can solve the inverse kinematics (IK) of the Panda robot. To solve IK, we will need the following:

The desired pose of the end effector (by default, this is the last link in the "panda_arm" chain): the end we calculated in the above steps_ effector_ state.
Timeout: 0.1 s

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

Now we can print out the IK solution (if found):

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

5. Get Jacobian determinant

We can also get Jacobian determinant from RobotState.

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);
ROS_INFO_STREAM("Jacobian: \n" << jacobian << "\n");

3, Startup file

To run the code, you need a startup file that can do two things:
Load Panda URDF and SRDF on the parameter server, and then set MoveIt to the kinematics generated by the assistant_ The solver configuration is placed on the ROS parameter server in the node namespace that instantiates the class in this tutorial.

<launch>
  <include file="$(find panda_moveit_config)/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
  </include>

  <node name="robot_model_and_robot_state_tutorial"
        pkg="moveit_tutorials"
        type="robot_model_and_robot_state_tutorial"
        respawn="false" output="screen">
    <rosparam command="load"
              file="$(find panda_moveit_config)/config/kinematics.yaml"/>
  </node>
</launch>

Topics: Ubuntu ROS

Programmer Think