Robot Model and Robot State¶
In this section, we will walk you through the C++ API for using kinematics in MoveIt.
The RobotModel and RobotState Classes¶
The RobotModel and RobotState classes are the core classes that give you access to a robot’s kinematics.
The RobotModel class contains the relationships between all links and joints including their joint limit properties as loaded from the URDF. The RobotModel also separates the robot’s links and joints into planning groups defined in the SRDF. A separate tutorial on the URDF and SRDF can be found here: URDF and SRDF Tutorial
The RobotState contains information about the robot at a certain point in time, storing vectors of joint positions and optionally velocities and accelerations. This information can be used to obtain kinematic information about the robot that depends on its current state, such as the Jacobian of an end effector.
RobotState also contains helper functions for setting the arm location based on the end effector location (Cartesian pose) and for computing Cartesian trajectories.
In this example, we will walk through the process of using these classes with the Panda.
Getting Started¶
If you haven’t already done so, make sure you’ve completed the steps in Getting Started.
Running the Code¶
All the code in this tutorial can be compiled and run from the
moveit_tutorials
package that you have as part of your MoveIt
setup.
Roslaunch the launch file to run the code directly from moveit_tutorials:
roslaunch moveit_tutorials robot_model_and_robot_state_tutorial.launch
Expected Output¶
The expected output will be in the following form. The numbers will not match since we are using random joint values:
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
Note: Don’t worry if your output has different ROS console format. You can customize your ROS console logger by following this blog post.
The Entire Code¶
The entire code can be seen here in the MoveIt GitHub project.
Start¶
Setting up to start using the RobotModel class is very easy. In general, you will find that most higher-level components will return a shared pointer to the RobotModel. You should always use that when possible. In this example, we will start with such a shared pointer and discuss only the basic API. You can have a look at the actual code API for these classes to get more information about how to use more features provided by these classes.
We will start by instantiating a RobotModelLoader object, which will look up the robot description on the ROS parameter server and construct a RobotModel for us to use.
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 that maintains the configuration of the robot. We will set all joints in the state to their default values. We can then get a JointModelGroup, which represents the robot model for a particular group, e.g. the “panda_arm” of the 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();
Get Joint Values¶
We can retrieve the current set of joint values stored in the state for the Panda arm.
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]);
}
Joint Limits¶
setJointGroupPositions() does not enforce joint limits by itself, but a call to enforceBounds() will do it.
/* 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"));
Forward Kinematics¶
Now, we can compute forward kinematics for a set of random joint values. Note that we would like to find the pose of the “panda_link8” which is the most distal link in the “panda_arm” group of the robot.
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");
Inverse Kinematics¶
We can now solve inverse kinematics (IK) for 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): end_effector_state that we computed in the step above.
- The 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");
}
Get the Jacobian¶
We can also get the Jacobian from the 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");
The Launch File¶
- To run the code, you will need a launch file that does two things:
- Loads the Panda URDF and SRDF onto the parameter server, and
- Puts the kinematics_solver configuration generated by the MoveIt Setup Assistant onto the ROS parameter server in the namespace of the node that instantiates the classes 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>
Debugging the Robot State¶
To aid in debugging robot state, a command-line tool will help introspect your system:
rosrun moveit_ros_planning moveit_print_planning_model_info
Open Source Feedback
See something that needs improvement? Please open a pull request on this GitHub page