def solid_body_translate_lin_acc(self, lin_acc, state, transform):
# TODO(buckbaskin): check dynamics notes
# Free Moving Rigid Body Kinematics
# A-p = A-o + w-dot x R_prime-p + w x (w x R_prime-p)
# We have A-p (the linear acceleration from the Imu)
# We are solving for A-o (the acceleration for the baselink frame)
# angular velocity is consistent across the rigid body (measured at Imu)
# angular acceleration (w-dot) is interpolated from velocity data
# R-p is the vector between the two (transform)
# known: A-p, w-dot, w, R-p
# solving for: A-o
# A-o = A-p - w-dot x R-p - w x ( w x R-p )
Ap = Vector.from_Vector3(lin_acc) # v is a state_model.Vector
position = transform[0]
r = Vector.from_tf_position(position)
Ap = lin_acc
w_dot = state.alpha()
w = state.omega()
Ao = (Ap
.minus(w_dot.cross(r))
.minus(w.cross(w.cross(r))))
return Ao.to_Vector3()
