def _readConfigAtParam(self, client, data):
qin = client.robot.getCurrentConfig()
qout = list()
for segment in self.joint_selection[0]:
qout.extend(qin[segment[0]:segment[1]])
if self.rootJointName is not None:
rootpos = client.robot.getJointPosition(self.rootJointName)
from hpp import Quaternion
q = Quaternion(rootpos[3:7])
# TODO although it is weird, the root joint may not be at
# position 0
qout[0:self.rootJointSizes[0]] = rootpos[0:3] + q.toRPY().tolist()
return Vector(qout)
def hppPoseToSotTransRPY(pose):
from hpp import Quaternion
q = Quaternion(pose[3:7])
return pose[0:3] + q.toRPY().tolist()