Yaskawa-Global/motoros2

Investigate how to best make use of F/T data on HC platforms

gavanderhoorn opened this issue · 16 comments

gavanderhoorn commented

The HC series of robots have built-in torque sensors in all joints and come with various collaborative features built-in.

Some of the data used for those features is accessible to M+ applications: either via specific M+ APIs or fi via M-registers.

We should investigate if we can, and if so, how to, make use of that data. Either to improve existing interfaces or to make new ones available, specifically for users of HC robots.

An example improvement could be to use the values reported by the torque sensors in JointState messages instead of the commanded torque we currently use to populate them. We could also see whether it'd be possible to publish the estimated F/T on the TCP as a geometry_msgs/Wrench.

For information on M-registers used on HC platforms for this sort of data refer to 181437-1CD (HW1484764) Collaborative Operation Instructions, specifically the Register List on pages 174 and 175 (10-1 and 10-2).

iydv commented

I have been investigating this for a while:

  1. I can obtain F/T values from registers. Unfortunately, it is not convenient to constantly send requests via service call, the values should be published on the topic.

  2. MotoPlus has functions to read F/T values directly. But it doesn't work for me and I cannot find any description of these functions in MotoPlus manuals. I have tried to use the following functions:

extern int mpFcsGetForceData(MP_FCS_ROB_ID rob_id, int coord_type, int uf_no, MP_FCS_SENS_DATA sens_data);
extern int mpFcsGetSensorChannelData(MP_FCS_SENS_ID sens_id, MP_FCS_SENS_DATA sens_data);

they always return an error when I call them as follows:

MP_FCS_SENS_DATA sens_data;
bzero(sens_data, MP_FCS_AXES_NUM);
int f_ret = mpFcsGetForceData(MP_FCS_R1ID, 0, 0, sens_data);

and

MP_FCS_SENS_DATA sens_data;
bzero(sens_data, MP_FCS_AXES_NUM);
int f_ret = mpFcsGetSensorChannelData(MP_FCS_SENS1ID, sens_data);

@ted-miller is there an updated manual for MotoPlus API? I have checked all available manuals on motoman page and did not find any description of these functions.

gavanderhoorn commented

@iydv: thanks for looking into this.

I can obtain F/T values from registers.

Are you reading the registers referenced by the documentation I mentioned in the OP?

Unfortunately, it is not convenient to constantly send requests via service call, the values should be published on the topic.

indeed. That's what "best make use of" in the issue title would investigate :)

For HC robots specifically, joints are equipped with torque sensors. This allows for both torque readings per joint, as well as derivation/estimation of F/T values at the flange/TCP.

One of the things we could do is make use of the torque sensor data and report it in the JointState messages we publish. (edit: perhaps not, see #184 (comment))

And the estimated F/T data could be published as a Wrench, as I write in the OP.

Reading M-registers does not need to go through ROS services, MotoROS2 can read those directly as we do in Ros_CtrlGroup_GetFBServoSpeed(..) fi for joint velocity:

motoros2/src/CtrlGroup.c

Lines 375 to 444 in 9f8d26e

//-------------------------------------------------------------------
// Get the corrected feedback pulse speed in pulse for each axis.
//-------------------------------------------------------------------
BOOL Ros_CtrlGroup_GetFBServoSpeed(CtrlGroup* ctrlGroup, long pulseSpeed[MAX_PULSE_AXES])
{
int i;
#ifndef DUMMY_SERVO_MODE
LONG status;
MP_IO_INFO registerInfo[MAX_PULSE_AXES * 2]; //values are 4 bytes, which consumes 2 registers
USHORT registerValues[MAX_PULSE_AXES * 2];
UINT32 registerValuesLong[MAX_PULSE_AXES * 2];
bzero(pulseSpeed, sizeof(long[MAX_PULSE_AXES]));
if (!ctrlGroup->speedFeedbackRegisterAddress.bFeedbackSpeedEnabled)
return FALSE;
for (i = 0; i < MAX_PULSE_AXES; i += 1)
{
registerInfo[i * 2].ulAddr = ctrlGroup->speedFeedbackRegisterAddress.cioAddressForAxis[i][0];
registerInfo[(i * 2) + 1].ulAddr = ctrlGroup->speedFeedbackRegisterAddress.cioAddressForAxis[i][1];
}
// get raw (uncorrected/unscaled) joint speeds
status = mpReadIO(registerInfo, registerValues, MAX_PULSE_AXES * 2);
if (status != OK)
{
Ros_Debug_BroadcastMsg("Failed to get pulse feedback speed: %u", status);
return FALSE;
}
for (i = 0; i < MAX_PULSE_AXES; i += 1)
{
//move to 32 bit storage
registerValuesLong[i * 2] = registerValues[i * 2];
registerValuesLong[(i * 2) + 1] = registerValues[(i * 2) + 1];
//combine both registers into single 4 byte value (0.0001 deg/sec or 1 um/sec)
double dblRegister = (registerValuesLong[(i * 2) + 1] << 16) | registerValuesLong[i * 2];
//convert to pulse/sec
if (ctrlGroup->axisType.type[i] == AXIS_ROTATION)
{
dblRegister /= 1.0E4; //deg/sec
dblRegister *= RAD_PER_DEGREE; //rad/sec
dblRegister *= ctrlGroup->pulseToRad.PtoR[i]; //pulse/sec
}
else if (ctrlGroup->axisType.type[i] == AXIS_LINEAR)
{
dblRegister /= 1.0E6; //m/sec
dblRegister *= ctrlGroup->pulseToMeter.PtoM[i]; //pulse/sec
}
pulseSpeed[i] = (long)dblRegister;
}
#else //dummy-servo mode for testing
MP_CTRL_GRP_SEND_DATA sData;
MP_SERVO_SPEED_RSP_DATA pulse_data;
mpGetServoSpeed(&sData, &pulse_data);
// assign return value
for (i = 0; i<MAX_PULSE_AXES; ++i)
pulseSpeed[i] = pulse_data.lSpeed[i];
#endif
return TRUE;
}

We could perhaps use the M-registers with joint torque sensor data here:

motoros2/src/CtrlGroup.c

Lines 446 to 472 in 9f8d26e

//-------------------------------------------------------------------
// Retrieves the absolute value (Nm) of the maximum current servo torque.
//-------------------------------------------------------------------
BOOL Ros_CtrlGroup_GetTorque(CtrlGroup* ctrlGroup, double torqueValues[MAX_PULSE_AXES])
{
MP_GRP_AXES_T dst_vel;
MP_TRQ_CTL_VAL dst_trq;
LONG status = 0;
int i;
bzero(torqueValues, sizeof(double [MAX_PULSE_AXES])); // clear result, in case of error
bzero(dst_trq.data, sizeof(MP_TRQCTL_DATA));
dst_trq.unit = TRQ_NEWTON_METER; //request data in Nm
bzero(&dst_vel, sizeof(MP_GRP_AXES_T));
status = mpSvsGetVelTrqFb(dst_vel, &dst_trq);
if (status != OK)
return FALSE;
for (i = 0; i < MAX_PULSE_AXES; i += 1)
{
torqueValues[i] = (double)dst_trq.data[ctrlGroup->groupNo][i] * 0.000001; //Use double. Float only good for 6 sig digits.
}
return TRUE;
}

or add an additional function that gets used on HC platforms.

gavanderhoorn commented

And the estimated F/T data could be published as a Wrench, as I write in the OP.

Note that geometry_msgs/msg/Wrench(Stamped) is already included in M+ libmicroros we distribute with MotoROS2. See the build_info.yaml included in the .zip.

iydv commented

Are you reading the registers referenced by the documentation I mentioned in the OP?

yes, I have also found this information before, since we have required reading F/T values for our application. I have been using read_mregister function to read TCP forces for several month and it works fine. However, it requires additional configuration of parameters on the robot.

If it is fine to implement publisher in MotoROS2 by reading M-registers, I can do that. Do you want to publish F/T data to separate topic?
And which data should be retrieved? Currently, following possible:

  1. Estimated TCP force: Fx, Fy, Fz, Mx, My, Mz and force resultant F
  2. Estimated external torque for each axis
  3. Raw sensor data for each axis from 2 channels.

Maybe it is good to publish all that information on 2 different topics:

  1. TCP + estimated external joint torque
  2. Raw sensor data split by channel.
gavanderhoorn commented

I'd suggest we try to stick to standard ROS messages and APIs as much as possible, at least for an initial implementation.

So:

Estimated TCP force: Fx, Fy, Fz, Mx, My, Mz and force resultant F

as a geometry_msgs/msg/WrenchStamped.

We'll have to investigate some conventions around topic(s) typically used for this sort of data (other robot drivers fi).

Let's keep the effort low for now, and prototype something around the WrenchStamped publisher.

We can then take a look at the rest of the available information later and decide how to best make that available to ROS applications.

I have been using read_mregister function to read TCP forces for several month and it works fine. However, it requires additional configuration of parameters on the robot.

we can probably embed / integrate the needed parameter changes in either the ParameterLib or MotoROS2 itself, such that this configuration is done automatically. We could potentially make this conditional on some setting in the MotoROS2 config .yaml.

gavanderhoorn commented

As to implementation, some initial thoughts:

  • this seems sufficiently 'distant' from other functionality we have so far to deserve being implemented in its own file
  • we probably need to detect whether the platform/robot is an HC series and only then activate the discussed publisher: perhaps GP_isPflEnabled() could be used for this, although technically it doesn't seem like a proper test (there could be other robots except HCs -- in the future -- which would pass this test (ie: have PFL) but not necessarily have the data we're discussing available) @ted-miller @EricMarcil ?
  • WrenchStampeds have a header with a frame_id. We might need to transform the reported F/T data before publication, or if it's already reported in a ROS-compatible frame, we should make sure to set frame_id to that frame (rN/base perhaps?)
  • I would suggest to not add any more (micro-)ROS(2) related fields to the CtrlGroup or Controller structs. Instead, you could perhaps follow the approach used for services:
    • create a structure to store the WrenchStamped instance
    • have an Initialize() and Cleanup() take care of setting up that message instance (although it mostly consists of primitive fields, so just the header.frame_id would need special care (as a string)
    • perhaps add a function which fulfils the role of the Trigger(..) function in the service implementations, but which obviously doesn't take a request nor a response, but instead reads the M-registers, converts units, populates the fieds of the WrenchStamped and publish(..)s it.
    • call that function in a suitable place (not sure where yet, but perhaps as part of the main JointState and RobotStatus publication task)

we can update this list if/when needed.

gavanderhoorn commented

Just thought of something:

Estimated external torque for each axis

we could perhaps publish this on a separate topic (so not joint_states) as a JointState message with just the name and effort fields populated.

According to docs.ros.org/api/sensor_msgs/msg/JointState the effort field should technically only be used to report:

  • the effort that is applied in the joint (Nm or N).

that seems like it should not be used to report "estimated external torque for each axis", at least not on the main joint_states topic.

Having that data available in a ROS application would still be valuable though, so we could see about making it available.

iydv commented

ok, I will try to implement the first version and we can discuss the details once I finish it.

  • 👍1
ted-miller commented

I have tried to use the following functions:

extern int mpFcsGetForceData(MP_FCS_ROB_ID rob_id, int coord_type, int uf_no, MP_FCS_SENS_DATA sens_data);
extern int mpFcsGetSensorChannelData(MP_FCS_SENS_ID sens_id, MP_FCS_SENS_DATA sens_data);

@iydv, these functions are for a very specific function, called MotoFit. It only works with a special addon board and specific sensor. (I'm not sure why they're published in the public documentation.)

we probably need to detect whether the platform/robot is an HC series and only then activate the discussed publisher: perhaps GP_isPflEnabled() could be used for this, although technically it doesn't seem like a proper test (there could be other robots except HCs -- in the future -- which would pass this test (ie: have PFL) but not necessarily have the data we're discussing available)

I don't see how PFL could be "certified safe" without the additional FT sensors in each joint.

gavanderhoorn commented

I have tried to use the following functions:
extern int mpFcsGetForceData(MP_FCS_ROB_ID rob_id, int coord_type, int uf_no, MP_FCS_SENS_DATA sens_data);
extern int mpFcsGetSensorChannelData(MP_FCS_SENS_ID sens_id, MP_FCS_SENS_DATA sens_data);

@iydv, these functions are for a very specific function, called MotoFit. It only works with a special addon board and specific sensor. (I'm not sure why they're published in the public documentation.)

they're not AFAIK.

They are listed in mpSrvoFcs.h.

we probably need to detect whether the platform/robot is an HC series and only then activate the discussed publisher: perhaps GP_isPflEnabled() could be used for this, although technically it doesn't seem like a proper test (there could be other robots except HCs -- in the future -- which would pass this test (ie: have PFL) but not necessarily have the data we're discussing available)

I don't see how PFL could be "certified safe" without the additional FT sensors in each joint.

could be, but I just don't like relying on indirect evidence.

I'd rather have something which conclusively tells me "this platform has the necessary sensors installed" (ie: feature detection) than "this is model X, which you should trust has feature Y available".

ted-miller commented

they're not AFAIK.

They are. I asked YEC about them a while back. Technically, that sensor could be used without MotoFit. So someone decided to put them in the public API.

gavanderhoorn commented

Technically, that sensor could be used without MotoFit. So someone decided to put them in the public API.

so would that mean we could detect whether a controller is configured with such a sensor and then publish that data on the same topic we're discussing above, even if the robot is not in the HC series?

gavanderhoorn commented

@iydv: just curious whether you've had any chance to look into what we discussed above.

Really just a friendly ping.

iydv commented

I have just finished testing the first version this week. Took me longer than I thought. See #188

  • 👍1
ted-miller commented

so would that mean we could detect whether a controller is configured with such a sensor and then publish that data on the same topic we're discussing above, even if the robot is not in the HC series?

I don't know if we could detect the presence of a sensor. But I don't think it's relevant. I've never seen one of these sensors be used outside of a MotoFit application.

It seems that they have even removed these API from the latest documentation.

gavanderhoorn commented

so would that mean we could detect whether a controller is configured with such a sensor and then publish that data on the same topic we're discussing above, even if the robot is not in the HC series?

I don't know if we could detect the presence of a sensor. But I don't think it's relevant. I've never seen one of these sensors be used outside of a MotoFit application.

hmm.

It seems that they have even removed these API from the latest documentation.

yes, that's what I meant with my #184 (comment).