Use shoulders' joint axes to activate the actuators

[RiccardoGrieco]

This issue is to use the shoulders' axes instead of the elbow ones to provide the haptic feedback. This is needed since, if the operator lift the shoulders/arms to hold an object, the torque on the elbows doesn't give useful measurements.

The new approach will be using the shoulder_pitch to drive the actuators on the biceps and triceps, and using the shoulder_roll to drive another actuator which will be placed the closest to the shoulder as possible.

[RiccardoGrieco]

Managing the config files in feature-use_shoulder_joints, which is built on top of #18 .

The configuration files have been added. The missing points are:

• [x] Add actuators on the shrug for the shoulder roll
• [x] Adjust the thresholds

[RiccardoGrieco]

Yesterday me, @GiulioRomualdi and @paolo-viceconte performed some tests to catch some baseline values for the thresholds of the joint torques.

We tied 1kg and 2kg weights to the arms, with the robot in the following 4 poses:

Expand to see the photos

### Pose 1  ### Pose 2  ### Pose 3  ### Pose 4 

With the poses 1 and 2 stressing on the elbows and the shoulders respectively, and the 3 and 4 representing more balanced cases, which seem also more similar to the we have during the teleoperation.

Here the dataset:

WeightRetargetingDataset.zip

[RiccardoGrieco]

Data has been analysed with the robot-log-visualizer (big kudos to @GiulioRomualdi @isorrentino @paolo-viceconte for this tool!).

Below the plots of the left arm shoulder_pitch, shoulder_roll and elbow torques in the 4 configurations:

The mapping between the joint numbers and their name is:	ID	Joint axis name
6	l_shoulder_pitch
7	l_shoulder_roll
8	l_shoulder_yaw
9	l_elbow
10	r_shoulder_pitch
11	r_shoulder_roll
12	r_shoulder_yaw
13	r_elbow

Below, the maximum and minimum values of the torques configurations using 2kg, retrieved via the embedded terminal in the visualizer.

Max values

Joint axes	l_shoulder_pitch	l_shoulder_roll	l_elbow	r_shoulder_pitch	r_shoulder_roll	r_elbow
P1_2kg	-8.481098	5.676914	5.203982	-9.735357	5.513503	5.734851
P2_2kg	-11.476421	6.147718	1.686401	-13.856432	4.404973	-0.516476
P3_2kg	-11.720084	3.975589	5.32944	-13.519208	4.152523	6.371521
P4_2kg	-14.386555	3.684032	4.209047	-15.359994	2.07332	6.87403

Min values

Joint axes	l_shoulder_pitch	l_shoulder_roll	l_elbow	r_shoulder_pitch	r_shoulder_roll	r_elbow
P1_2kg	-3.892491	2.60486	1.770559	-5.227692	2.100122	2.098561
P2_2kg	-5.332713	3.158807	0.531548	-7.460333	2.321438	-1.042707
P3_2kg	-5.58013	2.025536	2.040463	-7.525092	1.658604	2.517222
P4_2kg	-7.212212	1.690777	1.876582	-8.72768	0.535304	2.702029

[RiccardoGrieco]

I analyzed the the data from the last tests to see if I was able to adjust some of the thresholds:

Below, the script I used to get the minimum and maximum torques (can be useful for future analysis):

Expand to see the script

```python import numpy as np torque_data = np.array(data["robot_logger_device"]["joints_state"]["torques"]["data"][:,[6,7,9,10,11,13]]) min_torques = [str(x) for x in torque_data.min(axis=0)] max_torques = [str(x) for x in torque_data.max(axis=0)] print("| MIN | "+"|".join(min_torques)+"|") print("| MAX | "+"|".join(max_torques)+"|") ```

Here the results:

Expand to see the results

|1 |l_shoulder_pitch | l_shoulder_roll | l_elbow | r_shoulder_pitch | r_shoulder_roll | r_elbow | |------------|---------|------------------|-----------------|---------|------------------|-----| | MIN | 4.336901|3.063897|1.325183|-4.210675|1.875287|0.773796 | | MAX | -4.132931|3.205644|1.414328|-2.632921|2.870599|1.353489| |2 |l_shoulder_pitch | l_shoulder_roll | l_elbow | r_shoulder_pitch | r_shoulder_roll | r_elbow | |------------|---------|------------------|-----------------|---------|------------------|-----| | MIN | -11.211291|1.121094|-1.651026|-21.73198|-7.168961|-4.573618| | MAX | -0.181564|12.587516|117.820872|2.362519|5.878451|535.173728| |3 |l_shoulder_pitch | l_shoulder_roll | l_elbow | r_shoulder_pitch | r_shoulder_roll | r_elbow | |------------|---------|------------------|-----------------|---------|------------------|-----| | MIN | -1.810918|0.0|-2.21239|-7.177763|-0.223169|-1.812342| | MAX | 1.988071|3.359229|0.0|2.091091|4.455384|1.730825| |4 |l_shoulder_pitch | l_shoulder_roll | l_elbow | r_shoulder_pitch | r_shoulder_roll | r_elbow | |------------|---------|------------------|-----------------|---------|------------------|-----| | MIN | -8.694009|-9.359099|-7.568648|-16.127043|-5.727289|-1.430559| | MAX | 5.615533|9.857077|97.838256|4.046841|5.142373|276.971552| |5 |l_shoulder_pitch | l_shoulder_roll | l_elbow | r_shoulder_pitch | r_shoulder_roll | r_elbow | |------------|---------|------------------|-----------------|---------|------------------|-----| | MIN | -14.859734|-8.414913|-10.224823|-14.262151|-4.145365|-2.893459| | MAX | 6.790122|6.190291|60.215008|0.679059|6.336716|32.608512| |6 |l_shoulder_pitch | l_shoulder_roll | l_elbow | r_shoulder_pitch | r_shoulder_roll | r_elbow | |------------|---------|------------------|-----------------|---------|------------------|-----| | MIN | -20.298476|-5.611497|-0.10601|-14.49611|-6.604896|-79.790616| | MAX | 0.359588|7.368474|305.353888|-1.266736|7.043334|238.489456|

Since some of these values looked very strange, I plotted them:

dataset 1

dataset 2

dataset 3

dataset 4

dataset 5

dataset 6

Those strange spikes don't seem related to particular events. Below an example from dataset 6:

https://user-images.githubusercontent.com/26788384/153621123-8e21e744-6d48-463e-bd26-713f0bde0f65.mp4

cc @mebbaid @S-Dafarra @GiulioRomualdi

[S-Dafarra]

I wonder if this can affect: https://github.com/robotology/icub-tech-support/issues/1299

[RiccardoGrieco]

Switched to the 1st and 99th percentile instead of minimum and maximum to filter out those values:

Expand to see the changes in the script

```python import numpy as np torque_data = np.array(data["robot_logger_device"]["joints_state"]["torques"]["data"][:,[6,7,9,10,11,13]]) max_torques = [str(x) for x in np.percentile(a=torque_data,q=99, axis=0)] min_torques = [str(x) for x in np.percentile(a=torque_data,q=1, axis=0)] print("| MIN | "+"|".join(min_torques)+"|") print("| MAX | "+"|".join(max_torques)+"|") ```

Results

1	l_shoulder_pitch	l_shoulder_roll	l_elbow	r_shoulder_pitch	r_shoulder_roll	r_elbow
MAX	-4.188488	3.1520788	1.4011182	-3.0877	2.78520	1.2989
MIN	-4.29260	3.0917038	1.3647984	-4.1274022	1.982711	0.9274

2	l_shoulder_pitch	l_shoulder_roll	l_elbow	r_shoulder_pitch	r_shoulder_roll	r_elbow
MAX	-0.5009	4.85882712	2.1168816	0.6395	4.26601832	1.74897318
MIN	-8.0099	1.48993282	-0.77426	-7.3930	1.43016832	-2.63730692

3	l_shoulder_pitch	l_shoulder_roll	l_elbow	r_shoulder_pitch	r_shoulder_roll	r_elbow
MAX	0.6209717	3.2319394	0.0	1.4274917	3.4832201	1.5515837
MIN	-1.4246793	0.0	-1.1922813	-6.7308994	0.0	-1.2345327

4	l_shoulder_pitch	l_shoulder_roll	l_elbow	r_shoulder_pitch	r_shoulder_roll	r_elbow
MAX	3.0529942	5.930299	2.952629	3.870531	4.508666	2.3714294
MIN	-8.1879718	-2.9793279	-2.9701538	-9.0719762	-0.9857913	-1.2896899

5	l_shoulder_pitch	l_shoulder_roll	l_elbow	r_shoulder_pitch	r_shoulder_roll	r_elbow
MAX	-1.143929	5.595817	5.800694	-2.24347	4.904176	3.55418
MIN	-12.222373	-2.006179	-0.556044	-13.963958	-2.999877	-1.530663

6	l_shoulder_pitch	l_shoulder_roll	l_elbow	r_shoulder_pitch	r_shoulder_roll	r_elbow
MAX	-0.71784634	6.34986	3.104499	-1.81001205	6.2531	3.6536444
MIN	-18.189109	-4.570936	0.20773607	-7.86039668	-4.58645567	-1.87136158

I will be using these ones to update the thresholds

[RiccardoGrieco]

@Gianlucamilani made a new actuator that has been placed on the left shoulder of the shrug and I updated some thresholds based on the data above.

Closing the issue.