public RigidBodyTransform convertMocapPoseToRobotFrame(MocapRigidBody mocapRigidBody) { int id = mocapRigidBody.getId(); if(!mocapReferenceFrames.containsKey(id)) { ReferenceFrame mocapObjectFrame = createReferenceFrameForMocapObject(id); mocapReferenceFrames.put(id, mocapObjectFrame); } mocapRigidBody.getPose(mocapRigidBodyTransforms.get(id)); ReferenceFrame referenceFrame = mocapReferenceFrames.get(id); referenceFrame.update(); return referenceFrame.getTransformToDesiredFrame(mocapOffsetFrame); }
@Override public void updateRigidbodies(ArrayList<MocapRigidBody> listOfRigidbodies) { for(int i = 0; i < listOfRigidbodies.size(); i++) { MocapRigidBody mocapObject = listOfRigidbodies.get(i); int id = mocapObject.getId(); RigidBodyTransform pose = new RigidBodyTransform(); mocapObject.getPose(pose); pose = frameConverter.convertMocapPoseToRobotFrame(mocapObject); DetectedObjectPacket detectedMocapObject = new DetectedObjectPacket(pose, id); mocapModulePacketCommunicator.send(detectedMocapObject); } } }
mocapObject.getPose(pose); int id = mocapObject.getId();
private void sendDetectedObjectPacketToUi(MocapRigidBody mocapObject) { RigidBodyTransform pose = new RigidBodyTransform(); mocapObject.getPose(pose); if(USE_ROBOT_FRAME) { pose = mocapToStateEstimatorFrameConverter.convertMocapPoseToRobotFrame(mocapObject); } DetectedObjectPacket detectedMocapObject = new DetectedObjectPacket(pose, mocapObject.getId()); if(packetCommunicator.isConnected()) { packetCommunicator.send(detectedMocapObject); } } }
public void update(MocapRigidBody mocapObject) { if(enableMocapUpdates) { mocapObject.getPose(workingRigidBodyTransform); workingRigidBodyTransform.multiply(mocapJigCalibrationTransform); workingRigidBodyTransform.multiply(transformFromMocapHeadCentroidToHeadRoot); mocapHeadPoseInZUp.set(workingRigidBodyTransform); mocapHeadFrame.update(); robotDataReceiver.updateRobotModel(); mocapHeadFrame.getTransformToDesiredFrame(transformFromMocapHeadToRobotHead , robotHeadFrame); mocapOffsetFrame.update(); } }