概述
RobotModel描述了URDF和SRDF文件中包含的各种参数信息（如连杆参数、运动参数、运动参数上下限及连杆组定义等）。RobotStates包含机械臂当前状态信息（如关节角位移、角速度和角加速度等），可用于获取机械臂运动学指标。
示例代码
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2012, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Sachin Chitta, Michael Lautman*/
#include <ros/ros.h>
// MoveIt
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "robot_model_and_robot_state_tutorial");
ros::AsyncSpinner spinner(1);
spinner.start();
// BEGIN_TUTORIAL
// 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
// :moveit_core:`RobotModel` for us to use.
//
// .. _RobotModelLoader:
// http://docs.ros.org/melodic/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html
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 :moveit_core:`RobotModel`, we can construct a
// :moveit_core:`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
// :moveit_core:`JointModelGroup`, which represents the robot
// model for a particular group, e.g. the "panda_arm" of the Panda
// 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("panda_arm");
const std::vector<std::string>& joint_names = joint_model_group->getVariableNames();
// Get Joint Values
// ^^^^^^^^^^^^^^^^
// We can retreive 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 :moveit_core:`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");
// END_TUTORIAL
ros::shutdown();
return 0;
}