Introduction
As its name suggests, gravity compensation cancels out gravity's effect on a system. In our case, we compensate for gravity and joint frictions on an Interbotix arm with its joint motors. In this way, the arm can hold itself in its current position and still allow free movement with a small amount of force.
Why Do We Need This Feature?
This feature is useful when we use the arm as a leader arm in a teleoperation setting with a follower arm mimicking its motion, when we teach it a specific trajectory, and much more. In these scenarios, we prefer the arm to hold itself in position and allow us to move it around freely with a small magnitude of force. Failing to do this would require the user to manually fight against the gravity, leading to a much heavier laboring intensity, especially when the arm is heavy.
Gravity Compensation
Computing the torques needed to compensate for the gravity is a special case of the inverse dynamics problem where the gravity is the only external force. More specifically, one is interested in finding the following mapping
where we have the torques on the left and the robot model, joint position, velocity, and acceleration on the right.
One way to solve the inverse dynamics problem is the recursive Newton-Euler algorithm. It consists of three steps:
Compute the velocities and accelerations of the links
Compute the forces required to produce such motions, i.e., fictitious forces
Compute the forces acting upon the links
Step 1: the velocity and acceleration of the i'th link are given by the following recursive relations associated with the angular velocity and acceleration of the i'th joint and the i'th joint axis. Note that the i'th joint axis is determined by the current joint positions through forward kinematics.
Step 2: the fictitious force is associated with the inertia, velocity, and accelerations of the i'th link.
Step 3: the forces applied by the next joint, previous joint, and external sources on the i'th link should balance.
The resulting torque of the i'th joint is given by
The inverse dynamics solver used in the package is ported from the Orocos Kinematics and Dynamics Library (KDL). Please refer to the KDL doc page for its derivations and implementation details.
Friction Compensation
The joint frictions cause two problems:
The kinetic friction resists the joint from moving.
The disparity between the kinetic friction and the static friction causes stiction.
Making things worse, the frictions at the joints are mapped to a resisting wrench at the end effector by the force Jacobian matrix. Depending on the joint positions of the arm, the resulting wrench may become infinitely large.
We address this issue by using the motors to counteract the frictions proactively.
Kinetic Friction
When a joint is moving, we add a assisting torque towards the direction of the motion. We use the coulomb friction model where the friction force is proportional to the total normal force acting on the contact surface. And we simplify the nominal wrench transmitted through the joint into two parts:
A torque about the joint axis.
A constant wrench accounting for other forces and torques.
As a result, the kinetic friction torque about the joint axis is given by
Stiction
We add a dither to each joint when it is moving below a specified speed (i.e., dither_speed) to avoid stiction. It periodically pushes the joint in alternating directions so that the joint is moving for the majority of time. The dithering frequency is half of the publishing rate of the joint_states topic. The dithering magnitude matches that of the static friction torque, given by
Future Directions
Force feedback for teleoperation. Now that we can compute the torques needed to hold a robotic arm in position, given a leader arm and follower arm in a teleoperation setting, we can feed the remaining torques of the follower arm, which account for forces other than gravity, back to the controller of the leader arm, inducing extra torques proportional to the feedback. In this way, we can sense how much force is applied by the follower arm through the response of the leader arm. For example, we could hold an egg or plastic cup without crushing it.
Compensation beyond gravity and friction. If the arm is fixed on a mobile platform, the acceleration of the platform will induce fictitious force acting upon the arm, causing the arm to jerk in the opposite direction when the platform suddenly moves, just like what we sense when the car we sit in is suddenly accelerating or decelerating. If we have a quick and accurate estimation of the platform’s acceleration, we can take it into account simply by adding the platform acceleration to the gravitational acceleration.
Learn more
Usage on the Aloha kits: Mobile | Stationary
Usage on a single Interbotix arm