Investigate effects of sampling time on joint behavior

[PeterBowman]

I recall struggling with unusual behavior in, if I'm correct, velocity control mode regarding joints that move at different speeds even if given the same command. This was never reported, it just came to my mind after observing inconsistent configuration across all 28 TEO's joints. It is best to check the screenshots of the PID controller dialogue on EasySetUp to see what I mean (https://github.com/roboticslab-uc3m/teo-hardware-issues/issues/45#issuecomment-759550140).

Long story short, some joints perform position/speed control in the fast loop (same as torque control), some do that on the slow loop (10x slower than fast loop). Slow loop sampling periods vary between 0.5-1 ms. Now, the following information is based on the iPOS CANopen programming manual and was taken for granted by me:

• Object 207A h: Interpolated position 1 st order time (used in PV/PVT submodes): "(...) It is given in IU which is by default 0.8ms for steppers and 1ms for the other configurations."
• Object 6081 h: Profile velocity (used in position profile mode): "The default Sample loop is 1ms."
• Object 6069 h: Velocity sensor actual value (used in velocity profile mode): "If no factor is applied 65536 IU = 1 encoder increment / sample."

There are several references to sampling periods, there is a distinction made between steppers and other configurations, and a default value of 1 ms seems to prevail (#153 (comment)). I've got a hunch this value could actually depend on the internal configuration one can tweak via EasySetUp, thus internal calculations performed in our TechnosoftIpos device might be incomplete or wrong due to this missing factor.

I need to check this on the real hardware. In case my assumptions are not correct, let's investigate the real cause.

PS section 11.3 Speed profile example, step 6. Target velocity states: "By using a 500 lines incremental encoder and 1ms sample rate for position/speed control the corresponding value of object 60FF h expressed in encoder counts per sample is 140000 h."

PS2 see #198 (comment) (default fast loop sampling period?).

[PeterBowman]

Technosoft support has confirmed my assumptions:

Yes, the values are not fixed, they can be changed. Changing any of these values may alter the overall performance of your drive and must be done after consulting with us to see if it's necessary. Slow/fast loop sampling period values have been left editable for special applications.

It is stated in several places throughout the iPOS CANopen Programming User Manual (P091.063.iPOS.STO.UM.0219) that default sample loop is 1 ms for non-stepper motors, which is our case (cf. "8.2.2 Object 6081h: Profile velocity" and "9.2.7 Object 207Ah: Interpolated position 1st order time"). However, controller configuration in EasySetUp > "Drive Setup" > "Advanced" exposes an editable field for slow/fast loop sampling periods. Am I correct that the 1 ms sampling period stated in the user manual is not fixed and it actually depends on the values selected in this dialog (i.e. sampling frequency divider and whether position/speed control is executed either in fast or slow loop)? See attached screenshot.

[PeterBowman]

Candidates:

• Object 6081h: Profile velocity: setRefSpeedRaw + getRefSpeedRaw
• Object 6083h: Profile acceleration: setRefAccelerationRaw + getRefAccelerationRaw
• Object 60FFh: Target velocity: setControlModeRaw
• Object 201Ch: External On-line Torque Reference: setControlModeRaw not affected, torque/current is converted separately
• Object 60C1h: Interpolation data record (legacy PT/PVT submodes, see Offline position-direct implementation via PT/PVT #245)

[PeterBowman]

When confirmed, the pulsesPerSample motor parameter (example) would need to be moved and carefully particularized per each joint .ini (all config files). As I noted in #153 (comment), it is best to review our formulae and physical interpretation as well. For instance:

yarp-devices/libraries/YarpPlugins/TechnosoftIpos/StateVariables.cpp

Lines 278 to 281 in 31a3361

	double StateVariables::degreesToInternalUnits(double value, int derivativeOrder) const
	{
	return value * tr * (reverse ? -1 : 1) * (encoderPulses / 360.0) * std::pow(1.0 / pulsesPerSample, derivativeOrder);
	}

The result of the conversion into internal units should be expressed in terms of pulses (position), pulses per sample (velocity) or pulses per squared sample (acceleration). However, the naming of the pulsesPerSample variable doesn't make sense after a thorough inspection. It should be written as samplesPerSecond instead in order to ensure coherence in units, so "1000 pulses per 1 sample" would be actually read as "1000 samples per 1 second". In other words, this is equivalent to using a sampling frequency of 1000 Hz or a discretization period of 0.001 seconds.

[PeterBowman]

ID	name	slow loop (ms)	fast loop (ms)	high res speed estimator
1	RightSagittalAnkle	0.5	0.05	no
2	RightFrontalAnkle	1	0.1	no
3	RightFrontalKnee	1	0.1	yes
4	RightFrontalHip	1	0.1	no
5	RightSagittalHip	1	0.1	no
6	RightAxialHip	0.5	0.05	no
7	LeftAxialHip	1	0.1	no
8	LeftSagittalHip	1	0.1	no
9	LeftFrontalHip	1	0.1	no
10	LeftFrontalKnee	1	0.1	yes
11	LeftFrontalAnkle	1	0.1	no
12	LeftSagittalAnkle	0.5	0.05	no
13	AxialWaist	0.5	0.05	no
14	FrontalWaist	1	0.1	no
15	RightFrontalShoulder	1	0.1	yes
16	RightSagittalShoulder	1	0.1	yes
17	RightAxialShoulder	1	0.1	yes
18	RightFrontalElbow	1	0.1	yes
19	RightAxialWrist	1	0.1	no
20	RightFrontalWrist	1	0.1	no
21	LeftFrontalShoulder	1	0.1	yes
22	LeftSagittalShoulder	1	0.1	yes
23	LeftAxialShoulder	1	0.1	no
24	LeftFrontalElbow	1	0.1	yes
25	LeftAxialWrist	1	0.1	no
26	LeftFrontalWrist	1	0.1	no
27	AxialNeck	1	0.1	yes
28	FrontalNeck	1	0.1	yes

[PeterBowman]

I have confirmed joints 1, 6, 12 and 13 reach twice higher reference speeds/accelerations, so the table above holds true. I had issues with 9 and 23 due to CUI encoders timing out, but I'll assume their values are also correct. Keep in mind that the actual sampling time considered by iPOS drives is the slow loop period, regardless of position/speed loops being configured in fast loop mode (bolded).

I have used this script to plot velocities against time with a yarpscope: roboticslab-uc3m/teo-gazebo-models#1 (comment). However, I had applied 31cfd25 before because of a bug in the handling of internal units. Now, IMotorEncoders returns positions as encoder counts, speeds as encoder counts per sample, and accelerations as encoder counts per sample².

[PeterBowman]

Done at 98842dd and roboticslab-uc3m/teo-configuration-files@43357e5, changes applied on real HW.

[jgvictores]

Is the table of #253 (comment) something we would want/like in https://github.com/roboticslab-uc3m/teo-developer-manual ?

PD: https://github.com/roboticslab-uc3m/teo-developer-manual/tree/master/csv (perma)

[PeterBowman]

I'm not sure, it's just another controller parameter like Kp, Ki, Kd and we are not disclosing those anywhere (for external use).

[PeterBowman]

Keep in mind that the actual sampling time considered by iPOS drives is the slow loop period, regardless of position/speed loops being configured in fast loop mode (bolded).

Confirmed (kind of?) at MotionChip™ II TML Programming User Manual, 2.1.1 Position Profile Modes:

Related TML Variables
TPOS: Target position (long) – position reference computed by the reference generator at each slow loop (position/speed loop) sampling period when a position profile mode is performed. Measured in position units
TSPD: Target speed (fixed) – speed reference computed by the reference generator at each slow loop sampling period when a position profile mode is performed. Measured in speed units
TACC: Target acceleration (fixed) – acceleration/deceleration reference computed by the reference generator at each slow loop sampling period when a position profile mode is performed. Measured in acceleration units

Also at MotionChip™ II TML Configuration Setup User Manual, 3.4.2 Speed sensors:

The speed is estimated as the variation of the position over one sampling period of the speed / position control loop. This method can be implemented for any motion structure that has a position sensor defined.
Be aware that this method has a resolution depending on the number of equivalent lines per rotation, of the position sensor, and the value of the slow sampling period of the MotionChip II control module. Figure 3.3 presents the principle of the method.