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)
Exemple #2
0
def hppPoseToSotTransRPY(pose):
    from hpp import Quaternion
    q = Quaternion(pose[3:7])
    return pose[0:3] + q.toRPY().tolist()