MoveIt! 学习笔记3- MoveIt RobotModel and Robot State (读取机器人关节等信息，测试正解逆解是否可行)
此博文主要是用来记录ROS-Kinetic 中，用于机器人轨迹规划的MoveIt功能包的学习记录。
英文原版教程见此链接：http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/move_group_interface/move_group_interface_tutorial.html
引： MoveIt RobotModel Robot_State这个教程主要是用来检查URDF+SRDF文件内容，查看机器人关节限制，设置坐标，检测机器人是否能够通过正解和逆解的方式，达到预设坐标点；虽然在这个教程里，这些功能用来做测试，但是，在以后的项目中，可以用其作为参考，进行运动学正逆解规划测试等，很有学习必要。
注：MoveIt教程基本上都是通过官方案例程序，展示具体实现的过程和代码信息，所以本博文采用在官方程序中，添加中文注解的方式，进行学习和记录。
/********************************************************************** 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 // MoveIt!
#include
#include
#include int main(int argc, char** argv)
{//Step0: 创建ROS节点，并开启同步循环体ros::init(argc, argv, "robot_model_and_robot_state_tutorial");ros::AsyncSpinner spinner(1);spinner.start();// Step1： 读取机器人模型信息：创建RobotModelLoader对象，从ROS服务中读取"robot_description"内的机器人模型信息，// 之后创建一个RobotModelPtr对象， 将机器人模型存入其中。// .. _RobotModelLoader:// http://docs.ros.org/kinetic/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.htmlrobot_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());// Step2： 转存机器人当前状态：创建一个RobotStatePtr kinematic_state对象，并将上边加载有机器人模型数据的数值传入// 创建一个JointModelGroup* joint_model_group指针对象，读取model里面的JOintGroup// 创建一个String类型的向量，用来存储joint——grope内部个关节的名称// 注意！！重要！！ kinematic_state这个对象，是用来操作机器人运动等的核心// joint_model_group这个对象，设置的是机器人的运动组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& joint_names = joint_model_group->getVariableNames();// Step3： 转存机器人关节状态：创建一个double类型的向量，并从kinematic_state对象中，读取机器人各个关节的当前位置，并使用for循环遍历显示std::vector joint_values;kinematic_state->copyJointGroupPositions(joint_model_group, joint_values); //从前边input读取，存入后边的outputfor (std::size_t i = 0; i < joint_names.size(); ++i){ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);}// Step4：设置一个关节值，并确认这个关节值是否超出限制值？// 从上边已经存有各关节位置的vector对象中，单独设置其中1轴的转角。// 将设置完转角的新的关节Vector，设置到机器人的kinematic_state内部。// 使用下边两条语句，检测是否超限？ （经检查，超限）// 将当前关节数值，强制设置为机器人新的limitation，然后再次检查关节位置是否超限？（经检查，未超限）// ^^^^^^^^^^^^// 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"));// Step5：运动学正解： 首先，使用setToRandomPositions语句，设置一个随机的位置给：joint_model_group（也就是panda——arm）// 之后，创建一个Eigen::Affine3d&四元数类型对象，从kinematic_state中，读取设置完随机位置之后的，末端执行其的坐标// 显示最终的末端执行器的位置及转角 // 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::Affine3d& 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");// Step6：运动学逆解： 设置运动学逆解参数，并进行规划// 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 number of attempts to be made at solving IK: 10// * The timeout for each attempt: 0.1 sstd::size_t attempts = 10;double timeout = 0.1;bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, attempts, 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");}// Step7：获取机器人雅各比矩阵// ^^^^^^^^^^^^^^^^// 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_TUTORIALros::shutdown();return 0;
}
本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！