def moveHandModelToFrame(model, frame): pos, quat = transformUtils.poseFromTransform(frame) rpy = transformUtils.quaternionToRollPitchYaw(quat) pose = np.hstack((pos, rpy)) model.model.setJointPositions(pose, [ 'base_x', 'base_y', 'base_z', 'base_roll', 'base_pitch', 'base_yaw' ])
def onPoseBDI(self,msg): self.pose_bdi = msg # Set the xyzrpy of this pose to equal that estimated by BDI rpy = transformUtils.quaternionToRollPitchYaw(msg.orientation) pose = self.jointController.q.copy() pose[0:3] = msg.pos pose[3:6] = rpy self.bdiJointController.setPose("ERS BDI", pose)
def convertStateMessageToDrakePose(msg): jointMap = {} for name, position in zip(msg.joint_name, msg.joint_position): jointMap[name] = position jointPositions = [] for name in getDrakePoseJointNames()[6:]: jointPositions.append(jointMap[name]) trans = msg.pose.translation quat = msg.pose.rotation trans = [trans.x, trans.y, trans.z] quat = [quat.w, quat.x, quat.y, quat.z] rpy = transformUtils.quaternionToRollPitchYaw(quat) pose = np.hstack((trans, rpy, jointPositions)) assert len(pose) == getNumPositions() return pose
def getRollPitchYawFromRobotState(robotState): return transformUtils.quaternionToRollPitchYaw(robotState[3:7])
def moveHandModelToFrame(model, frame): pos, quat = transformUtils.poseFromTransform(frame) rpy = transformUtils.quaternionToRollPitchYaw(quat) pose = np.hstack((pos, rpy)) model.model.setJointPositions(pose, ['base_x', 'base_y', 'base_z', 'base_roll', 'base_pitch', 'base_yaw'])