Moveit Forward Kinematic shows nan result
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)
Asked by lcmai on 2019-12-13 01:30:00 UTC
Answers
I kind of figure out the solution:
Instead of computing forward kinematic from goal_state, I compute the forward kinemacit from robot_state, which is defined as
robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model));
and it works well.
I have no idea why this happen as the goal_state should be equivalent to the robot_state.
It would be great if someone can tell me the reason.
Asked by lcmai on 2019-12-13 05:29:53 UTC
Comments