JointRelayHandler calibration offsets support?

[krebsalad]

I am not sure if this is the right place to ask this.

I want to edit the messages sent to the controller, namely the joint angles. I want to add a certain offset to each joint angle based on the calibration tag in the robots URDF. I am assuming that this is currently not a feature in the driver, at least, I could not find something regarding this in the abb_driver nor in the industrial_robot_client. Can someone point me to where I can best add these offsets to the joint angles/joint states?

somewhere here? https://github.com/ros-industrial/industrial_core/blob/kinetic-devel/industrial_robot_client/src/joint_relay_handler.cpp#L98

[gavanderhoorn]

Context: https://github.com/mikeferguson/robot_calibration/issues/64 (please always include that in the future).

[gavanderhoorn]

Adding support for calibration offsets could be interesting.

somewhere here? https://github.com/ros-industrial/industrial_core/blob/kinetic-devel/industrial_robot_client/src/joint_relay_handler.cpp#98

That doesn't link me to a specific line number (this does) so I assume you are referring to JointRelayHandler::create_messages(..).

There are two sides to this:

1. messages coming from the driver server
2. messages going to the server

The first needs a transformation from SimpleMessage to the appropriate ROS message, while the second needs the inverse transformation.

For calibration to be taken into account properly (and I'm assuming you're talking about joint space offsets), you'd have to subtract the offsets in the first case, and add them in the second.

The bigger issue here I believe would be to make the offsets themselves available to the conversion functions without introducing all sorts of extra infrastructure.

[gavanderhoorn]

It would be nice if we could add this to industrial_robot_client itself, as that would add support for this to all robot drivers making use of that.

In this specific case though (the abb_driver), it would probably have to be added to ABB_JointRelayHandler as well, as that subclasses the regular JointRelayHandler. But whether that is really needed will depend on how things will be implemented exactly.

[krebsalad]

I assume you are referring to JointRelayHandler::create_messages(..)

Yes, I was referring to that function. Thanks for your confirmation.

For calibration to be taken into account properly (and I'm assuming you're talking about joint space offsets), you'd have to subtract the offsets in the first case, and add them in the second.

I will try implementing this in my project. I will report back here in the coming week.

There are two sides to this:

you'd have to subtract the offsets in the first case, and add them in the second.

So it would be something like this?

1. After selecting specific joints for publishing
2. Copy pub_joint_pos into two different vectors : case 1 and case 2
3. Somehow get joint offsets
4. Subtract joint offsets from case1 and add joint offsets to case2
5. assign vector case1 to control_state positions and vector case2 to sensor_state position

Or did i understand it the wrong way?

Also, should I change the title to something in the lines of: JointRelayHandler calibration offsets support?

[gavanderhoorn]

Or did i understand it the wrong way?

yes (you misunderstood), that is not what I meant.

The function you link to only works in one direction. You need to update the functions for both directions of data flow.

Also, should I change the title to something in the lines of: JointRelayHandler calibration offsets support?

yes, that would seem like a good thing to do.

I might even transfer this issue to the ros-industrial/industrial_core issue tracker, as it's not something specific to the abb_driver implementation.

[krebsalad]

The function you link to only works in one direction. You need to update the functions for both directions of data flow.

I looked for the other direction based on the nodes called from the abb_driver robot_interface.launch and found the joint_trajectory_downloader. With the other direction do you mean this function? https://github.com/ros-industrial/industrial_core/blob/kinetic-devel/industrial_robot_client/src/joint_trajectory_downloader.cpp#L42

So 1: messages coming from the driver server = in function create_messages(....) 2: message going to the server = in function send_to_robot(...)

So, again, to me it seems that the points, which have the joint data in function send_to_robot(..) are the values that need to be added with the joint offsets and the values in function create_messages(..) need to be subtracted with the offsets. Please correct me if I'm wrong.

I tried adding some lines in both joint_trajectory_downloader as joint_relay_handler.

joint_relay_handler.cpp

• after selecting specific joints for publishing
• Added hardcoded(for now) vector with offset values called joint_offsets.

  
  for(unsigned int iter = 0; iter < pub_joint_pos.size() && iter < 6; iter++) 
  {
  pub_joint_pos[iter] -= joint_offsets[iter];
  }

joint_trajectory_downloader.cpp:
- [under sending trajectory points log](https://github.com/ros-industrial/industrial_core/blob/kinetic-devel/industrial_robot_client/src/joint_trajectory_downloader.cpp#L62)
- Added hardcoded(for now) vector with offset values called joint_offsets.

// loop through all points for(unsigned int iter = 0; iter < points.size(); iter++) { // get joint data of point as jointData object industrial::joint_data::JointData jdata; points[iter].point.getJointPosition(j_data);

// set joint offsets into copy of jointData
for(industrial::shared_types::shared_int i = 0; i <  j_data.getMaxNumJoints(); i++)
{
  // get joint value
  industrial::shared_types::shared_real value;

  // loop through all joints (assuming they are in the same order as joint_offsets)
  if(j_data.getJoint(i, value) && joint_offsets[i] != NULL)
  {
      // set joint value with offset
      value += joint_offsets[i];
      j_data.setJoint(i, value);
  }
  else
  {
    break;
  }
}
//set new joint data (only the positions)
points[iter].point_.setJointPosition(j_data);

}

I tried this implementation with a simulated robot. I worked fine for me. Am I missing anything? 

[krebsalad]

After further inspection, I don't think this particular implementation, with regards to joint offsets, would work efficiently for industrial robots. This, because the offset generally differs per pose. I'm not a professional in the ways industrial robots are used. But my understanding is that most users make use of the high repeatability. Whatsmore, vendors develop the robots with that in mind. These are the reasons I believe the addition is not required in the industrial core until a better approach comes around.

Though, i do believe that an addition where the user can transform the joint angles before sending to the robot and receiving will be useful for future development. For example, having an inheriting class implementing the functionality. In my case that would be in a class in abb_driver, where I can put the above code. To this end, some inheritable functions that get called in send_to_robot and create_messages are required. This way the current code doesn't get hurt, only passing the joint angles or trajectory one more time should be fine for now. Similar to transform function in joint_trajectory_interface, which doesn't do anything when not implemented by a child class.

Any thoughts on this?