Kinematics/C++ API ================== In this section, we will walk you through the C++ API for using kinematics. The RobotModel and RobotState classes ------------------------------------- The :moveit_core:`RobotModel` and :moveit_core:`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. .. tutorial-formatter:: ../kinematic_model_tutorial.cpp The entire code ^^^^^^^^^^^^^^^ The entire code can be seen :codedir:`here in the moveit_pr2 github project`. Compiling the code ^^^^^^^^^^^^^^^^^^ Follow the `instructions for compiling code from source `_. The launch file ^^^^^^^^^^^^^^^ To run the code, you will need a launch file that does two things: * Uploads the PR2 URDF and SRDF onto the param 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. .. literalinclude:: ../../launch/kinematic_model_tutorial.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. Running the code ^^^^^^^^^^^^^^^^ Roslaunch the launch file to run the code directly from pr2_moveit_tutorials:: roslaunch pr2_moveit_tutorials kinematic_model_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:: [ 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 Additional Reading ^^^^^^^^^^^^^^^^^^ * `RobotState Display `_ - Visualization of the RobotState using Rviz Links ^^^^^ * :moveit_core:`RobotModel` Code API * :moveit_core:`RobotState` Code API * Back to :moveit_website:`Moveit Tutorials `