def axis_angle_to_rotation(axis, angle):
# Source:
# https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation
res = cs.MX.eye(3)
res += cs.sin(angle) * cs.skew(axis)
res += (1 - cs.cos(angle)) * cs.mtimes(cs.skew(axis), cs.skew(axis))
return res
def spatial_force_transform(R, r):
"""Returns the spatial force transform from a 3x3 rotation matrix
and a 3x1 displacement vector."""
X = cs.SX.zeros(6, 6)
X[:3, :3] = R.T
X[3:, 3:] = R.T
X[:3, 3:] = cs.mtimes(cs.skew(r), R.T)
return X
def spatial_transform(R, r):
"""Returns the spatial motion transform from a 3x3 rotation matrix
and a 3x1 displacement vector."""
X = cs.SX.zeros(6, 6)
X[:3, :3] = R
X[3:, 3:] = R
X[3:, :3] = -cs.mtimes(R, cs.skew(r))
return X
