标签：INFO Joint robot joint state cpp model panda
英文解释网站
代码注释
#include <ros/ros.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
/**
* RobotModel class: 包含连杆和关节间的关系、关节限制属性、规划组
* RobotState class: 包含机器人在某个瞬时快照中的信息，储存关节位置的vector以及可选择的关节速度和加速度，
* 可用于获取机器人的运动学信息，该运动学信息取决于机器人的当前状态，例如末端效应器的雅可比矩阵
*/
int main(int argc, char **argv){
ros::init(argc, argv, "robot_model_and_robot_state_tutorial");
ros::AsyncSpinner spinner(1);
spinner.start();
/* 实例化一个RobotModelLoader对象，该对象能够在ROS参数服务器上查找机器人描述文件和构建一个RobotModel供用户使用 */
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());
/* 使用RobotModel构建一个RobotState，RobotState维护了机器人的配置。我们将要设置所有的关节为默认值。
然后创建JointModelGroup，能够表示特定规划组的机器人模型，例如panda机器人的panda_arm */
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();
/* 获得关节值 */
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_values[0] = 5.57;
kinematic_state->setJointGroupPositions(joint_model_group, joint_values);
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));
kinematic_state->enforceBounds();
ROS_INFO_STREAM("Current state is " << (kinematic_state->satisfiesBounds() ? "valid" : "not valid"));
/* 正运动学 */
kinematic_state->setToRandomPositions(joint_model_group);
const Eigen::Affine3d& end_effector_state = kinematic_state->getGlobalLinkTransform("panda_link8");
ROS_INFO_STREAM("Translation: \n" << end_effector_state.translation() << "\n");
ROS_INFO_STREAM("Rotation: \n" << end_effector_state.rotation() << "\n");
/* 逆运动学 */
std::size_t attempts = 10;
double timeout = 0.1;
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, attempts, timeout);
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");
}
/* 对于给定的规划组，参照给定连杆上的特定点计算雅可比行列式 */
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");
ros::shutdown();
return 0;
}
运行结果
[ INFO] [1613378520.695315909]: Loading robot model 'panda'...
[ INFO] [1613378520.805682066]: Loading robot model 'panda'...
[ INFO] [1613378520.819762498]: Model frame: /world
[ INFO] [1613378520.819980696]: Joint panda_joint1: 0.000000
[ INFO] [1613378520.820021362]: Joint panda_joint2: 0.000000
[ INFO] [1613378520.820032424]: Joint panda_joint3: 0.000000
[ INFO] [1613378520.820040063]: Joint panda_joint4: -1.570800
[ INFO] [1613378520.820065712]: Joint panda_joint5: 0.000000
[ INFO] [1613378520.820102894]: Joint panda_joint6: 0.000000
[ INFO] [1613378520.820140727]: Joint panda_joint7: 0.000000
[ INFO] [1613378520.820224733]: Current state is not valid
[ INFO] [1613378520.820281047]: Current state is valid
[ INFO] [1613378520.820444452]: Translation:
0.410832
0.324039
0.968244
[ INFO] [1613378520.820607828]: Rotation:
-0.117757 -0.280658 0.952557
-0.861389 -0.448415 -0.238606
0.494107 -0.84862 -0.188951
[ INFO] [1613378520.821023450]: Joint panda_joint1: -2.594111
[ INFO] [1613378520.821070674]: Joint panda_joint2: -0.665205
[ INFO] [1613378520.821081967]: Joint panda_joint3: -1.195657
[ INFO] [1613378520.821103151]: Joint panda_joint4: -0.311509
[ INFO] [1613378520.821124815]: Joint panda_joint5: -2.776861
[ INFO] [1613378520.821156191]: Joint panda_joint6: 1.560667
[ INFO] [1613378520.821166102]: Joint panda_joint7: 1.632811
[ INFO] [1613378520.821374549]: Jacobian:
-0.324039 -0.542396 -0.0508554 0.352593 0.0108034 -0.0530715 -1.04083e-17
0.410832 -0.330669 -0.0115386 -0.334732 0.0897549 0.0745977 -3.46945e-17
0 0.519459 0.0387756 -0.137787 -0.0588788 0.103979 -6.93889e-18
0 0.520538 0.527006 0.434347 0.277752 -0.100137 0.952557
0 -0.853838 0.321287 0.693921 0.503329 -0.831943 -0.238606
1 4.89664e-12 0.786791 -0.574296 0.818238 0.545751 -0.188951
标签：INFO,Joint,robot,joint,state,cpp,model,panda 来源： https://blog.csdn.net/Jesse_Liu666/article/details/113938299
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。