参考Kinematic Model Tutorial
RobotModel 与 RobotState类是访问运动学的核心类。这个例子中我们使用PR2的右臂来走通使用这两个类的过程。
初始化一个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可以构建一个RobotModel变量,保存机器人的配置信息。我们设机器人各个关节为默认值。可以得到一个JointModelGroup表示机器人的运动组,比如“right_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("right_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]);
}
/* 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"));
通过一组随机的关节值实现正向运动学。
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());
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");
}
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);
启动文件之前要先做两件事:
<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="moveit_tutorials"
type="kinematic_model_tutorial"
respawn="false" output="screen">
<rosparam command="load"
file="$(find pr2_moveit_config)/config/kinematics.yaml"/>
node>
launch>
roslaunch moveit_tutorials kinematic_model_tutorial.launch