In this section, we will walk you through the C++ API for using kinematics.
The RobotModel and RobotState classes are the core classes that give you access to the kinematics. In this example, we will walk through the process of using the classes for the right arm of the PR2.
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");
robot_model::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 “right_arm” of the PR2 robot.
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("right_arm");
const std::vector<std::string> &joint_names = joint_model_group->getJointModelNames();
We can retreive the current set of joint values stored in the state for the right 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]);
}
setJointGroupPositions() does not enforce joint limits by itself, but a call to enforceBounds() will do it.
/* Set one joint in the right arm outside its joint limit */
joint_values[0] = 1.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"));
Now, we can compute forward kinematics for a set of random joint values. Note that we would like to find the pose of the “r_wrist_roll_link” which is the most distal link in the “right_arm” of the robot.
kinematic_state->setToRandomPositions(joint_model_group);
const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("r_wrist_roll_link");
/* Print end-effector pose. Remember that this is in the model frame */
ROS_INFO_STREAM("Translation: " << end_effector_state.translation());
ROS_INFO_STREAM("Rotation: " << end_effector_state.rotation());
We can now solve inverse kinematics (IK) for the right arm of the PR2 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 “right_arm” chain): end_effector_state that we computed in the step above.
- The number of attempts to be made at solving IK: 5
- The timeout for each attempt: 0.1 s
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 10, 0.1);
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");
}
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: " << jacobian);
The entire code can be seen here in the moveit_pr2 github project.
Follow the instructions for compiling code from source.
<launch>
<include file="$(find pr2_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<node name="kinematic_model_tutorial"
pkg="pr2_moveit_tutorials"
type="kinematic_model_tutorial"
respawn="false" output="screen">
<rosparam command="load"
file="$(find pr2_moveit_config)/config/kinematics.yaml"/>
</node>
</launch>
All the code in this tutorial can be compiled and run from the pr2_moveit_tutorials package that you have as part of your MoveIt! setup.
Roslaunch the launch file to run the code directly from pr2_moveit_tutorials:
roslaunch pr2_moveit_tutorials kinematic_model_tutorial.launch
The expected output will be in the following form. The numbers will not match since we are using random joint values:
[ INFO] [1384819451.749126980]: Model frame: /odom_combined
[ INFO] [1384819451.749228320]: Joint r_shoulder_pan_joint: 0.000000
[ INFO] [1384819451.749268059]: Joint r_shoulder_lift_joint: 0.000000
[ INFO] [1384819451.749298929]: Joint r_upper_arm_roll_joint: 0.000000
[ INFO] [1384819451.749337412]: Joint r_upper_arm_joint: -1.135650
[ INFO] [1384819451.749376593]: Joint r_elbow_flex_joint: 0.000000
[ INFO] [1384819451.749404669]: Joint r_forearm_roll_joint: -1.050000
[ INFO] [1384819451.749480167]: Joint r_forearm_joint: 0.000000
[ INFO] [1384819451.749545399]: Joint r_wrist_flex_joint: 0.000000
[ INFO] [1384819451.749577945]: Joint r_wrist_roll_joint: 0.000000
[ INFO] [1384819451.749660707]: Current state is not valid
[ INFO] [1384819451.749723425]: Current state is valid
[ INFO] [1384819451.750553768]: Translation: 0.0464629
-0.660372
1.08426
[ INFO] [1384819451.750715730]: Rotation: -0.0900434 -0.945724 -0.312248
-0.91467 -0.0455189 0.40163
-0.394045 0.321768 -0.860926
[ INFO] [1384819451.751673044]: Joint r_shoulder_pan_joint: -1.306166
[ INFO] [1384819451.751729266]: Joint r_shoulder_lift_joint: 0.502776
[ INFO] [1384819451.751791355]: Joint r_upper_arm_roll_joint: 0.103366
[ INFO] [1384819451.751853793]: Joint r_upper_arm_joint: -1.975539
[ INFO] [1384819451.751907012]: Joint r_elbow_flex_joint: 2.805000
[ INFO] [1384819451.751963863]: Joint r_forearm_roll_joint: -1.851939
[ INFO] [1384819451.752015895]: Joint r_forearm_joint: -0.414720
[ INFO] [1384819451.752064923]: Joint r_wrist_flex_joint: 0.000000
[ INFO] [1384819451.752117933]: Joint r_wrist_roll_joint: 0.000000
[ INFO] [1384819451.753252155]: Jacobian: 0.472372 0.0327816 -0.28711 0.0708816 1.38778e-17 0 0
0.0964629 -0.120972 -0.0626032 -0.311436 0 0 0
0 -0.381159 -0.0266776 -0.0320097 8.67362e-19 0 0
0 0.965189 0.229185 0.973042 -0.0665617 -0.991369 -0.0900434
0 0.261552 -0.845745 0.212168 -0.116995 0.12017 -0.91467
1 0 -0.48186 0.0904127 0.990899 -0.0524048 -0.394045
- RobotState Display - Visualization of the RobotState using Rviz
- RobotModel Code API
- RobotState Code API
- Back to Moveit Tutorials