Rosanswers logo

I am working on a robotics project using Ubuntu 18.04 and ROS Melodic, and I am using an Intel RealSense D435 camera to detect objects. I have been trying to use object pose estimation to pick up an object using Moveit, but I am encountering issues when using the 3D coordinates (x, y, z) set as a pose_target, the IK solver is unable to find a solution.

Specifically, I have tried getting the object's 2D center pixel values and converting them into a 3D coordinate with depth involved. However, when I set the 3D coordinates to a geometry_msgs::Pose target_pose and pass it to setFromIK(joint_model_group, target_pose), the IK solver is unable to find a solution. I suspect that my conversions may be incorrect. How can I properly convert the 2D pixel coordinates to 3D coordinates, and why is the IK solver unable to find a solution with the target_pose that I have provided?

/* Convert pixel to 3d coordinate */ 
   ray = model_.projectPixelTo3dRay(cv::Point2d(p_2d.x, p_2d.y));
   // Coordinates affected due to depth
   double alpha = depth_value/ray.z;
   projected_dl.center.x = -ray.y*alpha;
   projected_dl.center.y = -ray.x*alpha;
   projected_dl.center.z = ray.z*alpha;
/* ============================= */
  geometry_msgs::Pose target_pose;
  target_pose.position.x = projected_dl.center.x;
  target_pose.position.y = projected_dl.center.y ;
  target_pose.position.z = projected_dl.center.z;

  bool found_ik = kinematic_state->setFromIK(joint_model_group, target_pose);

  // 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]);
    ROS_INFO("Did not find IK solution");

Originally posted by RowBot on ROS Answers with karma: 35 on 2023-04-24

Post score: 2


1 Answer 1


Rosanswers logo

From the description of the scenario, the problem may arise from the fact that you are giving a point inside the object as final goal of for the end-effector. If the robot is building an occupancy map of the environment with the camera you mentioned, it will consider everything as an obstacle (including your object), if not instructed differently.

You could verify if this is the case by setting a similar goal, but in a non-occupied spot (e.g. 0.5m above the position you would have defined). Does the solver return you a solution in this case?

Originally posted by bluegiraffe-sc with karma: 221 on 2023-05-22

This answer was ACCEPTED on the original site

Post score: 0

Original comments

Comment by RowBot on 2023-05-23:
Hey thanks for answering, I have tried the suggested before and it does not work. But I notice that my object detection coordinates from my camera is different from my end-effector position at the object.

Here is an example: Camera Object Detection Coordinates: (x: -1.3414, y: -0.0301, z: 0.4340), End-effector Position at the object: (x: 0.3146, y: 0.0702, z: 0.0157)

I have been following this as a guide.

Comment by bluegiraffe-sc on 2023-05-29:
Sorry, I try to figure out the situation. You mean that:

  • if you print the target_pose, you get (x: -1.3414, y: -0.0301, z: 0.4340)
  • if you (manually?) move the end-effector to the desired position, and print its position, you get (x: 0.3146, y: 0.0702, z: 0.0157)

If the above is correct, you are missing a transformation between reference frames (RF). You have the coordinates of the object in the camera RF (typically Z forward, X right, and Y down), but your planner is expecting to receive a target in a RF aligned with the base of the robot, e.g. base_link (usually, X forward, Y right, and Z up).

As per the documentation of the method you are trying to use, The pose is assumed to be in the reference frame of the kinematic model. You should use tf2 to convert the position you get from the camera RF to the relevant RF.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.