def sot_2_pinocchio(q):
# PINOCCHIO Free flyer 0-6, CHEST HEAD 7-10, LARM 11-17, RARM 18-24, LLEG 25-30, RLEG 31-36
# SOT Free flyer 0-5, RLEG 6-11, LLEG 12-17, CHEST HEAD 18-21, RARM 22-28, LARM 29-35
qPino = np.matlib.zeros((37, 1))
qPino[:3, 0] = q[:3]
quatMat = rpyToMatrix(q[3:6])
quatVec = Quaternion(quatMat)
qPino[3:7, 0] = quatVec.coeffs()
qPino[7:11, 0] = q[18:22]
# chest-head
qPino[11:18, 0] = q[29:]
# larm
qPino[18:25, 0] = q[22:29]
# rarm
qPino[25:31, 0] = q[12:18]
# lleg
qPino[31:, 0] = q[6:12]
# rleg
return qPino
def rotateFromRPY(placement, rpy):
trans = SE3.Identity()
trans.rotation = rpyToMatrix(rpy)
return placement.act(trans)