On the Gravity Functions

This article talks about the various gravity functions and how to use them.

ImageIn normal operation, if the flotor is being levitated and servoed to a position/orientation, for example (0,0,0,0,0,0), in 6 DOFs using the built-in PD control laws, there will  be small errors in all six degrees of freedom x = x, y, z, θ, Φ, ψ  due to gravity, gravity torques, and other small effects. This is because stable operation can only be achieved with finite values of proportional gains Kp.

Most of the time, these small errors can be neglected.  However, if the magnetic levitation haptic device is re-oriented in its work surface, for example, to a more comfortable attitude for the user, the previous feed-forward gain set 6-vector  Kp may not correctly act to cancel gravity.
The figure shows a procedure for automatically finding the direction and magnitude of the gravity vector. The position and orientation errors can be canceled by computing the proper feed-forward gain set Kf . In the first block, the average position and orientation 6-vector avg(x) is computed by summing over N loops of execution, where N = 50 is nominal. In the second block, the position and orientation error 6-vector δx from the desired value xd is computed. In the next block, the 6-vector of feed-forward control gains Kf is incremented by a small amount proportional to the error δx, causing the flotor to move. After waiting for several ms as shown in the block Wait for motion to settle, a test is made in the next block to determine if the error is less than a small pre-defined amount ε. The procedure iterates until finishing in the final block, when gravitational forces and torques are effectively canceled.
The API provides four functions, ml_FindGravity(), ml_SetGravity(), ml_GetGravity(), and ml_DefyGravity() for dealing with these small gravity-induced errors.  Their usage is rather confusing, so listen up!
ml_FindGravity() finds the feedforward forces and torques required to cancel gravitational forces and torques at the current flotor position and orientation, nominally (0,0,0,0,0,0). This function takes two arguments. The first argument dev_hdl is the device handle. The second argument gravity is a pointer to where the discovered feedforward forces and torques will be written.  This is the function implemented in the flowchart depicted above.  The function blocks and does not return to the calling program until finished, requiring several seconds to complete.  During execution, the user's hand must not be in contact with the device handle, which will understandably cause erroneous results.  The flotor will remain in its previous position and orientation upon exiting.
ml_SetGravity() is used to define a gravity vector in the current user coordinate frame. This function takes two arguments. The first argument is the device handle. The second argument gravity is a pointer to a 6-vector to use as the new gravity vector.  ml_SetGravity() can be used, for whatever reason, to over-ride the gravity vector found by ml_FindGravity().  If the user's application requires that the flotor have some weight, then one or more components of gravity can be modified as needed by adding in the weight.

ml_GetGravity() simply reads the gravity vector currently being used by the controller. This function takes two arguments. The first argument is the device handle. The second argument gravity is the address into which the read gravity vector will be stored.

Finally, ml_DefyGravity() causes the controller to set feedforward forces and torques for the Normal gainset equal to the previously stored gravity vector. This function takes one argument, the device handle.

Proper use: Use the ml_FindGravity() function to  calculate the actual current gravity vector, and then use ml_SetGravity() to set the gravity vector returned by ml_FindGravity(), and finally call ml_DefyGravity() to cause the flotor to begin resisting  gravity forces and torques.