Ask Your Question
0

Electric get_fk forward kinematics fails simple test

asked 2011-07-29 00:29:42 -0500

Pi Robot gravatar image

Hello ROS Gurus,

I am trying to get the new Electric arm navigation stack working on a pair of custom 6-dof Dynamixel arms (see pi_robot description). The planning wizard worked like a charm and I have two kinematics chains, one for each arm, both with root link "lower_torso_link" and tip link "left_hand_link" and "right_hand_link" respectively. The only tweak I made to the resulting launch files was to substitute my joint trajectory action controller (thanks to Anton Rebgun's dynamixel_controller package).

The resulting pi_robot_arm_navigation.launch file fires up without errors. Then I tried the simple get_fk.cpp sample from here but with the appropriate substitutions for my robot's left arm. (See my code listing below). When I run the resulting get_fk binary, I get the error:

[ERROR] [1311941555.891867066]: Forward kinematics service call failed

and in the terminal window where I launch the arm navigation nodes, I find that the pi_robot_left_arm_kinematics node has died:

[pi_robot_left_arm_kinematics-3] process has died [pid 10452, exit code -11].
log files: /home/patrick/.ros/log/b194841a-b998-11e0-bf54-8c736e77238f/pi_robot_left_arm_kinematics-3*.log

I can run the same get_fk.cpp file against David Lu's generic arm_kinematics package and it gives me back the FK solution without error.

Here now is my test get_fk.cpp file:

#include <ros/ros.h>
#include <kinematics_msgs/GetKinematicSolverInfo.h>
#include <kinematics_msgs/GetPositionFK.h>

int main(int argc, char **argv){
  ros::init (argc, argv, "get_fk");
  ros::NodeHandle rh;

  ros::service::waitForService("/pi_robot_left_arm_kinematics/get_fk_solver_info");
  ros::service::waitForService("/pi_robot_left_arm_kinematics/get_fk");

  ros::ServiceClient query_client = 
    rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>
    ("/pi_robot_left_arm_kinematics/get_fk_solver_info");
  ros::ServiceClient fk_client = rh.serviceClient
    <kinematics_msgs::GetPositionFK>("/pi_robot_left_arm_kinematics/get_fk");

  // define the service messages
  kinematics_msgs::GetKinematicSolverInfo::Request request;
  kinematics_msgs::GetKinematicSolverInfo::Response response;
  if(query_client.call(request,response))
    {
      for(unsigned int i=0; 
      i< response.kinematic_solver_info.joint_names.size(); i++)
    {
      ROS_INFO("Joint: %d %s", i,
            response.kinematic_solver_info.joint_names[i].c_str());
    }
    }
  else
    {
      ROS_ERROR("Could not call query service");
      ros::shutdown();
      exit(1);
    }
  // define the service messages
  kinematics_msgs::GetPositionFK::Request  fk_request;
  kinematics_msgs::GetPositionFK::Response fk_response;
  fk_request.header.frame_id = "lower_torso_link";
  fk_request.fk_link_names.resize(1);
  //fk_request.fk_link_names[0] = "left_shoulder_pan_link";
  //fk_request.fk_link_names[1] = "left_shoulder_lift_link";
  //fk_request.fk_link_names[2] = "left_upper_arm_link";
  //fk_request.fk_link_names[2] = "left_elbow_link";
  //fk_request.fk_link_names[3] = "left_wrist_link";
  //fk_request.fk_link_names[4] = "left_hand_link";
  fk_request.fk_link_names[0] = "left_hand_link";
  fk_request.robot_state.joint_state.position.resize
    (response.kinematic_solver_info.joint_names.size());
  fk_request.robot_state.joint_state.name = 
    response.kinematic_solver_info.joint_names;
  for(unsigned int i=0; 
      i< response.kinematic_solver_info.joint_names.size(); i++)
    {
      fk_request.robot_state.joint_state.position[i] = 0;
    }
  fk_request.robot_state.joint_state.position[0] = 1.00;
  if(fk_client.call(fk_request, fk_response))
    {
      if(fk_response.error_code.val == fk_response.error_code.SUCCESS)
    {

      for(unsigned int i=0; i < fk_response.pose_stamped.size(); i ++)
        {
          ROS_INFO_STREAM("Link    : " << fk_response.fk_link_names[i].c_str());
          ROS_INFO_STREAM("Position: " << 
                  fk_response.pose_stamped[i].pose.position.x << "," <<  
                  fk_response.pose_stamped[i].pose.position.y << "," << 
                  fk_response.pose_stamped[i].pose.position.z);
          ROS_INFO("Orientation: %f %f %f %f",
               fk_response.pose_stamped[i].pose.orientation.x,
               fk_response.pose_stamped[i].pose.orientation.y,
               fk_response.pose_stamped[i].pose.orientation.z,
               fk_response.pose_stamped[i].pose.orientation.w);
        } 
    }
      else
    {
      ROS_ERROR("Forward kinematics failed");
    }
    }
  else
    {
      ROS_ERROR("Forward kinematics service call failed");
    }
  ros::shutdown();
}
edit retag flag offensive close merge delete

2 answers

Sort by ยป oldest newest most voted
1

answered 2011-07-29 11:27:38 -0500

egiljones gravatar image

The program shouldn't segfault - that's a bug, and it will be fixed for the next release, which should be coming out in the next week or so.

What's going on is that pretty much all of the generated nodes depend on getting the planning scene from the environment server - this happens under the hood from the points of the tools that you've been using, and when you are programmatically accessing the ik node the planning scene has not been set. I won't get too far into the details, but the planning scene helps divorce the nodes themselves from the current state of the world - they are instead provided state using this planning scene mechanism. This means that you can use the nodes even without simulation, and even plan or solve ik for states that aren't the current state of the robot. Your pi_robot_arm_navigation.launch brings up the environment_server in a mode where it will supplement whatever planning scene you set with the current planning scene, but nothing is ever actually setting the planning scene at all and the get_fk call isn't robust to that, which is a bug.

To get things working before the release you can add the following lines to your test file (in the appropriate places):

//add before main
static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";

    // add before you call any kinematics services     
    ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);
    ros::ServiceClient set_planning_scene_diff_client = rh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff>(SET_PLANNING_SCENE_DIFF_NAME);


    arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
    arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;

    if(!set_planning_scene_diff_client.call(planning_scene_req, planning_scene_res)) {
      ROS_WARN("Can't set planning scene");
      return;
    }

If you set the planning scene before you call get_fk then things should work. This will set the planning scene to the current robot state.

The arm_kinematic_constraint_aware node should have more functionality in the get_fk and get_ik (non-constraint aware) service calls even if you haven't set a planning scene - I'll make sure that gets into the next release.

And all of this system design stuff will get posted to the wiki in short order - sorry for the early adoption pain.

edit flag offensive delete link more

Comments

Many thanks Gil--your workaround above did the trick. (And I even think I understand your explanation.) Happy to suffer a little pain in exchange for such a huge payoff. Thanks for all your hard work on arm navigation. Pi Robot ( 2011-07-29 13:59:17 -0500 )edit

Excuse me, Gil and Pi Robot, I got exactly the same problem, but Gil's trick above does not work, any suggestions please? PS: arm_navigation was installed with: sudo apt-get install ros-fuerte-arm-navigation. Thanks.

tor ( 2012-05-20 21:49:27 -0500 )edit

exactly the same problem. Gil's trick doesn't work neither. more error information from the terminal running arm navigation:Incomplete robot state in setPlanningScene; Setting planning scene state to NULL

yangyangcv ( 2012-08-31 02:13:20 -0500 )edit

Well, the question & solution above was for Electric. But (at least) yangyangcv is using Fuerte. Check, if you are providing all necessary joint states. Also, there is a bunch of similar Q&As here, which might help you.

bit-pirate ( 2012-09-02 13:43:29 -0500 )edit
1

@bit-pirate: well, gil's trick works for the PR2 robot on Fuerte, but it doesn't work for my own robot :(. there are indeed some posts about this problem, but it seems few of them are solved.

yangyangcv ( 2012-09-02 21:39:51 -0500 )edit
1

answered 2012-11-23 05:15:28 -0500

dbworth gravatar image

updated 2012-11-23 23:16:30 -0500

@yangyangcv @bit-pirate :

For FK, I was getting the same segfault with Fuerte. You should apply this patch.

Originally reported 4 months ago: https://code.ros.org/trac/ros-pkg/ticket/5497/

And by Boris: https://code.ros.org/trac/ros-pkg/ticket/5567/

For IK, I was getting "inverse kinematics failed".

You need to set the planning scene to null, as per Gil's code above.

You'll need to include an extra header file too:

#include <arm_navigation_msgs/SetPlanningSceneDiff.h>
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

[hide preview]

Stats

Asked: 2011-07-29 00:29:42 -0500

Seen: 820 times

Last updated: Nov 23 '12