Torque sensing

[JamesNewton]

If we know the stepper angle (meaning the angle the stepper motor has been told to go), as well as the actually measured angle #12 and there is little to no mechanical backlash #7 then we can accurately measure the torque on each joint. This can give Dexter an accurate sense of touch, which allows sensing collisions, the location of the workspace, and other objects.

Even now, we could move to a point, zero out the error, then move to another point, and watch the error until it exceeds a threshold, indicating a contact. If the two points are fairly close together, backlash, changes in gravitational pull and other issues are unlikely to influence the feedback.

[JamesNewton]

The torque on the joint in Newton meters (Τ) is the difference between the STEPPER_ANGLE and the MEASURED_ANGLE (Δ θ), times a "stiffness" constant (k) in Newton meters / degree.

Τ = k ⋅ Δθ

The STEPPER_ANGLE corresponds to the Move all Joints command in DDE. Using the BASE (Joint 1) as an example, is BASE_POSITION_AT + BASE_POSITION_PID_DELTA + BASE_POSITION_FORCE_DELTA + PLAYBACK_BASE_POSITION + FINE_ADJUST_BASE.

FINE_ADJUST_BASE is a value that we set in via DexRun

BASE_MEASURED_ANGLE will be returned in place of PLAYBACK_BASE_POSITION.

[JamesNewton]

It is critical to understand that the output of the PID is NOT a drive "strength" signal as one might have with a PWM output to a DC motor drive or a ESC driving a BLDC motor. The output is some number of step pulses per second telling the stepper motor to move to a new location, which then applies force to correct the error. The "_DELTA" positions are always going to be run all the way to zero if the stepper driver has the strength to do so. There is NO remaining error, unlike what there would be in a more traditional PID servo system using only the P term.

[JamesNewton]

Note: Ensure FPGA address 78 bit0 is ZERO via the write oplet to allow the PID_DELTA values to change and indicate how much corrective position adjustment is being applied. If we know how much it has corrected to hold position, we can calculate the torque.

[JamesNewton]

The gateware added the ability to read the step angles on the "Step Angle" branch, and that was merged into the the "Stable_Conedrive" branch which will be the next standard. An example job showing how to use this should be added here, and then this issue can be closed as soon as the new branch is in production.

[JamesWigglesworth]

This example shows a method of measuring torques and forces with Dexter from DDE The current (8/26/20) method for measuring torques makes a few assumptions and approximations that may cause inaccuracies:

-As of now Dexter's joints can only measure total torque which will include the contribution of robot's own weight and accelerations. We can make an approximation by "zeroing out" the torques while it's unloaded and before it starts moving. This is an approximation because when the robot moves each joint's center of mass will move and contribute to the torque. (This is why you will see non-zero and changing values before it touches anything) We have just completed characterizing each links center of mass so this addition may be coming soon. https://github.com/HaddingtonDynamics/Dexter/wiki/Dynamics#center-of-mass-measurements

-The torque measurement assumes that the torque calibration done at one angle holds true for all angles. I suspect that because we calibrate the encoder joint angle while under the effects of gravity, the difference between motor angle and encoder angle may already have some torque information biasing it. This is mostly an unknown just due to a lack of testing time.

A proposed solution to this would be to calibrate the robot's J2, J3, and J4 encoders with the robot on its side so that these joints are not affected by gravity. The torque due to gravity can then be calculated from each link's angle and center of mass and removed from the torque measurement.

Right now if J2 was 90 degrees and everything else was at 0 the robot would report 0 torque for J2.

[JamesNewton]

Added a new setting for the stiffness of each joint: S JointStiffness <ratio> <joint>; Parameters are a ratio (unit-less) and a joint number.

        case 59: //JointStiffness
            if (a3 > 0 && a3 <= NUM_JOINTS) {
                a3--; //convert to zero index
                printf("J%d stiffness was %f", a3+1, JointStiff[a3]);
                JointStiff[a3] = fa2;
                printf(" now %f\n", fa2);
                return 0;
                }
            break;

Example entry from Defaults.make_ins:

; Set joint stiffness ratios for this robot.
S JointStiffness 12.895, 1; Joint 1 stiffness
S JointStiffness 12.895, 2; Joint 2 stiffness
S JointStiffness 1.2568, 3; Joint 3 stiffness
S JointStiffness 0.1503, 4; Joint 4 stiffness
S JointStiffness 0.1503, 5; Joint 5 stiffness

[JamesNewton]

Kamino cloned this issue to HaddingtonDynamics/OCADO