Moveit Forward Kinematic shows nan result

edit

Hi, All:

I am new to ROS and I am trying to get familiar with it by following the tutorials. Currently, I am working on a MoveIt! Motion Planning API tutorial and I found a small problem with the demonstration code and I try to fix it.

The problem I found is that in the sub-tutorial for the pose Goal, the goal 2 is not show properly in the rviz because the pose is not update with respect to the joint goal. The original is as follow:

// Joint Space Goals
// ^^^^^^^^^^^^^^^^^
// Now, setup a joint space goal
robot_state::RobotState goal_state(robot_model);
std::vector<double> joint_values = { -1.0, 0.7, 0.7, -1.5, -0.7, 2.0, 0.0 };
goal_state.setJointGroupPositions(joint_model_group, joint_values);
moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);
req.goal_constraints.clear();
req.goal_constraints.push_back(joint_goal);

// Call the planner and visualize the trajectory
/* Re-construct the planning context */
context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
/* Call the Planner */
context->solve(res);
/* Check that the planning was successful */
if (res.error_code_.val != res.error_code_.SUCCESS)
{
ROS_ERROR("Could not compute plan successfully");
return 0;
}
/* Visualize the trajectory */
res.getMessage(response);
display_trajectory.trajectory.push_back(response.trajectory);

/* Now you should see two planned trajectories in series*/
visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
visual_tools.trigger();
display_publisher.publish(display_trajectory);

/* We will add more goals. But first, set the state in the planning
 scene to the final state of the last plan */
robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
planning_scene->setCurrentState(*robot_state.get());

// Display the goal state
visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
visual_tools.publishAxisLabeled(pose.pose, "goal_2");
visual_tools.publishText(text_pose, "Joint Space Goal (2)", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();

/* Wait for user input */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

I then try to solve the problem by computing the forward kinematic of the goal_state and then display it on the rviz. The codes I try to add are as follow:

// we should update the pose w.r.t the set joint value. //
  geometry_msgs::PoseStamped goal2_pose;
  const Eigen::Affine3d& end_effector_state = goal_state.getGlobalLinkTransform("panda_link8");
  Eigen::Matrix3d rot = end_effector_state.rotation();
  ROS_INFO_STREAM("Translation: \n" << end_effector_state.translation() << "\n");
  ROS_INFO_STREAM("Rotation: \n" << rot << "\n");
  Eigen::Quaterniond rot_q(rot);
  goal2_pose.pose.position.x = end_effector_state.translation().x();
  goal2_pose.pose.position.y = end_effector_state.translation().y();
  goal2_pose.pose.position.z = end_effector_state.translation().z();

  goal2_pose.pose.orientation.x = rot_q.x();
  goal2_pose.pose.orientation.y = rot_q.y();
  goal2_pose.pose.orientation.z = rot_q.z();
  goal2_pose.pose.orientation.w = rot_q.w();

  // Display the goal state
  visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
  visual_tools.publishAxisLabeled(goal2_pose.pose, "goal_2");
  visual_tools.publishText(text_pose, "Joint Space Goal (2)", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

But I got an unexpected result which are

[INFO] [1576216582.328683651]: Translation: 
0.62422
   -nan
   -nan

[INFO] [1576216582.328788526]: Rotation: 
-nan -nan -nan
-nan -nan -nan
-nan -nan -nan

So my question is why I got this result. Thank you so much for your help. (btw: I am working with ROS kinetic)