Ask Your Question
0

Can only perform kinematic computation for a single link (simple question)

asked 2014-04-25 00:05:53 -0600

paturdc gravatar image

updated 2014-04-25 00:40:39 -0600

I made an arm_navigation stack using the Planning Description Configuration Wizard. However, when I try to run forward kinematics on my robot, only a single link shows up in the GetKinematicSolverInfo service response.

[ INFO] [1398420068.098256875]: Joint: 0 shoulder_pan_joint
[ INFO] [1398420068.098394147]: Joint: 1 shoulder_lift_joint
[ INFO] [1398420068.098505827]: Joint: 2 elbow_joint
[ INFO] [1398420068.098577658]: Joint: 3 wrist_1_joint
[ INFO] [1398420068.098646898]: Joint: 4 wrist_2_joint
[ INFO] [1398420068.098723696]: Joint: 5 wrist_3_joint
[ INFO] [1398420068.098798122]: 
[ INFO] [1398420068.098873428]: Link: 0 wrist_3_link

Did I make a mistake in the Planning Configuration Wizard, or must I look at the setup for my planner? There doesn't seem to be much precise information in the relevant tutorials.

EDIT: It could be that wrist_3_link is the only link that the solver will take as input, but if I try to solve for the forward kinematics for wrist_3_link, the kinematics server crasher. I assumed it was because I haven't specified the all the links, but that may not be the case. Perhaps there is a bug in the code, although I cannot find it.

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

class ur5_listener_class
{
private: 
  sensor_msgs::JointState ur5_state;

public:
  void returnJointState(const sensor_msgs::JointState msg)
  {
    // The callback function only passes on the data to the class
    ur5_state = msg;
  }

  sensor_msgs::JointState applyJointState()
  {
    // if the joint_state data is needed in the main function, it will be returned here.
    return ur5_state;
  }

  void debug()
  {
    for(int i=0;i<6;i++)
    {
      std::cout << "The " << ur5_state.name.at(i) << " is now at " << ur5_state.position.at(i);
      std::cout << " moving with velocity " << ur5_state.velocity.at(i) << "\n";
    }
  }
};

int main(int argc, char **argv){
  // Variable declaration   
  sensor_msgs::JointState ur5_state;

  ros::init (argc, argv, "get_fk");
  ros::NodeHandle rh;

    // Before other business, connect to node, and retrieve joint state
  ros::Subscriber sub;
  ur5_listener_class ur5_listener;
  sub = rh.subscribe("/joint_states", 1000, &ur5_listener_class::returnJointState, &ur5_listener);
  // Wait for the connection to establish
  ros::Rate r(10);
  while (sub.getNumPublishers() == 0)
  r.sleep();

  // retrieve joint_state and use for JointGoal
  ros::spinOnce();
  ur5_state = ur5_listener.applyJointState();

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

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

  // define the service messages
  kinematics_msgs::GetKinematicSolverInfo::Request request;
  kinematics_msgs::GetKinematicSolverInfo::Response response;
  if(query_client.call(request,response))
  {
    ROS_INFO("number of joints on service %i",
        response.kinematic_solver_info.joint_names.size());
    ROS_INFO("number of links on service %i",
        response.kinematic_solver_info.link_names.size());
    ROS_INFO("Link in question: %s",
        response.kinematic_solver_info.link_names[0].c_str());
    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 = "base_link";
  fk_request.fk_link_names.resize(1);
  fk_request.fk_link_names[0] = "wrist_3_link";
  ROS_INFO("Forward request call made");
  fk_request.robot_state.joint_state.position.resize
    (response.kinematic_solver_info.joint_names.size());
    // ROS_INFO("joint name resized");
  fk_request.robot_state.joint_state.name = 
    response.kinematic_solver_info.joint_names;
    // ROS_INFO("Joint names passed");
  for ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
1

answered 2014-04-25 00:45:47 -0600

updated 2014-04-25 00:46:44 -0600

The fact that only one link shows up is expected. That link is your IK link, i.e. the link that you can compute IK (inverse kinematics) for. The result will be a joint state for each of the 6 listed joints. Vice versa for FK (forward kinematics): you have to give the FK service a joint state for your 6 joints and will receive a resulting pose of your wrist_3_link.

edit flag offensive delete link more

Comments

Thanks. There must be some bug in the code then.

paturdc gravatar imagepaturdc ( 2014-04-25 00:48:26 -0600 )edit

Your code looks good at first glance. Maybe you could try fk_request.robot_state.joint_state.position[i] = 0.0 (if 0.0 is a valid position for all your joints), just to make sure your requested position is reachable.

Martin Günther gravatar imageMartin Günther ( 2014-04-25 00:55:22 -0600 )edit

If that also fails (and you're sure your requested joint state is a valid configuration), the problem is with the configuration of the IK plugin you're using, or your URDF.

Martin Günther gravatar imageMartin Günther ( 2014-04-25 00:56:42 -0600 )edit

I'm using the actual joint_state, so it should be valid. Is there something special you need to do to configure the kinematics plugin? I am only using the autogenerated arm_navigation.launch file, from the Planning Description Wizard. It has a kinematics node, which I assumed was enough.

paturdc gravatar imagepaturdc ( 2014-04-25 01:37:15 -0600 )edit

it is the one called arm_kinematics_constraint_aware/KDLArmKinematicsPlugin

paturdc gravatar imagepaturdc ( 2014-04-25 02:04:37 -0600 )edit

IIRC, you don't need to configure anything special for the KDL kinematics plugin. The crash is definitely a bug. The KDL plugin parses your URDF, so maybe there is something in the URDF that it doesn't like.

Martin Günther gravatar imageMartin Günther ( 2014-04-25 02:28:37 -0600 )edit

Yes, in fact it crashes when I run the PR2 tutorial, for forward kinematics as well. I guess I should make a new question about that.

paturdc gravatar imagepaturdc ( 2014-04-25 02:33:10 -0600 )edit

Your Answer

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

Add Answer

Question Tools

1 follower

Stats

Asked: 2014-04-25 00:05:53 -0600

Seen: 85 times

Last updated: Apr 25 '14