Revision history [back]

Starting with this tutorial, I wrote a standalone c++ app that calculates forward kinematics for my robot.

The following is a snippet that includes the nearby relevant code from the tutorial with my changes in the middle.

// straight from the tutorial
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());
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));

// this part is different - input any joint values and receive cartesian position as output
double current_pos[] = {0,0,0,0,0,0};
ROS_INFO("Joint pos = [ %f, %f, %f, %f, %f, %f ]", current_pos[0], current_pos[1], current_pos[2], current_pos[3], current_pos[4], current_pos[5] );
kinematic_state->setVariablePositions(current_pos);
kinematic_state->update();

// straight from the tutorial
const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("link_6");
ROS_INFO_STREAM("Translation: " << end_effector_state.translation());
ROS_INFO_STREAM("Rotation: " << end_effector_state.rotation());

Starting with this tutorial, I wrote a standalone c++ app that calculates forward kinematics for my robot.

The following is a snippet that includes the nearby relevant code from the tutorial with my changes in the middle.

// straight from the tutorial
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());
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));

// this part is different - input any joint values and receive cartesian position as output
double current_pos[] = {0,0,0,0,0,0};
ROS_INFO("Joint pos = [ %f, %f, %f, %f, %f, %f ]", current_pos[0], current_pos[1], current_pos[2], current_pos[3], current_pos[4], current_pos[5] );
kinematic_state->setVariablePositions(current_pos);
kinematic_state->update();

// straight from the tutorial
const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("link_6");
ROS_INFO_STREAM("Translation: " << end_effector_state.translation());
ROS_INFO_STREAM("Rotation: " << end_effector_state.rotation());