Static equilibrium for 7 dof manipulator

Question

I have a 7 dof manipulator (Kuka LBR4+) and would like to calculate the joint torques needed to keep the arm in a static equilibrium. In most books the transposed jacobian is used to map the forces applying on the end effector to the joint torques.

$\tau = J^T\cdot F$

That however doesn't take the mass of the links into account. Is there a way to calculate the needed torques for a given configuration so that, assuming an ideal case, by setting these torques the arm will be in a static equilibrium?

cheers

EDIT:

For everybody interested, i found a solution to this problem in Introduction to Robotics - Third Edition by John J. Craig on Page 175-176. It is done with the aid of the iterative Newton-Euler dynamics algorithm. The actual trick is, to set all velocities and accelerations to zero except for the base acceleration. The base acceleration will be $^0 \dot v_0 = G$, where G has the magnitude of the gravity vector but points in opposite direction. This is equivalent to saying that the base of the robot is accelerating upwards with 1 g and this upward acceleration causes exactly the same effect on the link as gravity would.

Answer 1

This is something that i have looked for in standard textbooks and have not found much information on it. I am not sure if it is because they are geared towards industrial arms which are typically not backdrivable. (meaning once you reach a position, you can essentially turn off your motors and the arm will continue to hold position). Or if they don't care about this level of control. (meaning they have low-level controllers that handle it and the arm will "know what to do"). Or if it is too simple of a problem to worry about.

nevertheless, i wrote some code to calculate these torques for a simulated robot. the output seems correct, although it was never tested. This is python code for OpenRAVE.

# Calculate the center of mass past the current joint.
# This assumes the mass is specified properly for each link.
def get_COM_past_joint(joint):
    mass = 0.0
    com = np.array([0.0, 0.0, 0.0])
    for i in range(joint.GetDOFIndex(), robot.GetDOF()):
        j = robot.GetJointFromDOFIndex(i)
        link = j.GetHierarchyChildLink()
        mass += link.GetMass()
        com += link.GetMass() * link.GetGlobalCOM()
    # todo: add mass in robot's hand
    com /= mass    
    return (mass, com)

# Calculate the torque required to hold joints in current configuration.
# Procedure:
#  1. calculate the center of mass "past" the joint
#  2. find the closest perpendicular distance between the COM and joint axis
#  3. cross the joint axis with gravity vector
#  4. cross the perpendicular distance vector with gravity vector
#  5. return mass * direction * magnitude of both crosses 
def get_static_joint_torque(joint):
    (mass, com) = get_COM_past_joint(joint)
    anchor = joint.GetAnchor()
    axis = joint.GetAxis()
    joint_to_com_closest = (anchor - com) - np.dot((anchor - com), axis) * axis
    axis_cross_k = np.cross(axis, np.array([0, 0, -1]))
    joint_to_com_closest_cross_k = np.cross(joint_to_com_closest, np.array([0, 0, -1]))
    direction = np.sign(np.dot(axis, joint_to_com_closest_cross_k))
    return mass*direction*np.linalg.norm(axis_cross_k)*np.linalg.norm(joint_to_com_closest_cross_k)

EDIT: I recently found out that OpenRAVE does a more advanced form of this calculation already. (And their units are correct too).

robot.SetDOFValues(dofvalues)
robot.SetDOFVelocities(dofvelocities)
torques = robot.ComputeInverseDynamics(dofaccelerations)

or

torqueconfiguration, torquecoriolis, torquegravity = robot.ComputeInverseDynamics(dofaccelerations,None,returncomponents=True)

Answer 2

If you represent the links by their center of mass you should be able to calculate the torque applied at each joint by the weight of the arm. Then your tau on the left hand side would be the tau_control that you're setting minus the tau_links which is the torques placed on each joint by the links, i.e. tau = tau_control - tau_links.