private MocapRigidBody getPelvisRigidBody(ArrayList<MocapRigidBody> listOfRigidbodies) { for (int i = 0; i < listOfRigidbodies.size(); i++) { MocapRigidBody rigidBody = listOfRigidbodies.get(i); if (rigidBody.getId() == PELVIS_ID) return rigidBody; } return null; }
@Override public void updateRigidbodies(ArrayList<MocapRigidBody> listOfRigidbodies) { for (MocapRigidBody rb : listOfRigidbodies) { if (rb.getId() == rigidBodyIdToTrack) { System.out.println(rb.toString()); } } } }
@Override public void updateRigidbodies(ArrayList<MocapRigidBody> listOfRigidbodies) { for (MocapRigidBody rb : listOfRigidbodies) { if (rb.getId() == rigidBodyIdToTrack) { System.out.println(rb.toString()); } } } }
public RigidBodyTransform convertMocapPoseToRobotFrame(MocapRigidBody mocapRigidBody) { int id = mocapRigidBody.getId(); if(!mocapReferenceFrames.containsKey(id)) { ReferenceFrame mocapObjectFrame = createReferenceFrameForMocapObject(id); mocapReferenceFrames.put(id, mocapObjectFrame); } mocapRigidBody.packPose(mocapRigidBodyTransforms.get(id)); ReferenceFrame referenceFrame = mocapReferenceFrames.get(id); referenceFrame.update(); return referenceFrame.getTransformToDesiredFrame(mocapOffsetFrame); }
public RigidBodyTransform convertMocapPoseToRobotFrame(MocapRigidBody mocapRigidBody) { int id = mocapRigidBody.getId(); if(!mocapReferenceFrames.containsKey(id)) { ReferenceFrame mocapObjectFrame = createReferenceFrameForMocapObject(id); mocapReferenceFrames.put(id, mocapObjectFrame); } mocapRigidBody.packPose(mocapRigidBodyTransforms.get(id)); ReferenceFrame referenceFrame = mocapReferenceFrames.get(id); referenceFrame.update(); return referenceFrame.getTransformToDesiredFrame(mocapOffsetFrame); }
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); }
public ScsMocapRigidBody(MocapRigidBody mocapRigidBody) { this(mocapRigidBody.getId(), mocapRigidBody.getPosition(), mocapRigidBody.getOrientation(), mocapRigidBody.getListOfAssociatedMarkers(), mocapRigidBody.dataValid); }
public ScsMocapRigidBody(MocapRigidBody mocapRigidBody) { this(mocapRigidBody.getId(), mocapRigidBody.getPosition(), mocapRigidBody.getOrientation(), mocapRigidBody.getListOfAssociatedMarkers(), mocapRigidBody.dataValid); }
@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); } } }
private void sendDetectedObjectPacketToUi(MocapRigidBody mocapObject) { RigidBodyTransform pose = new RigidBodyTransform(); mocapObject.packPose(pose); if(USE_ROBOT_FRAME) { pose = mocapToStateEstimatorFrameConverter.convertMocapPoseToRobotFrame(mocapObject); } DetectedObjectPacket detectedMocapObject = new DetectedObjectPacket(pose, mocapObject.getId()); if(packetCommunicator.isConnected()) { packetCommunicator.send(detectedMocapObject); } } }
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); } } }
@Override public void updateRigidbodies(ArrayList<MocapRigidBody> listOfRigidbodies) { if(!mainNode.isStarted()) return; for (MocapRigidBody rigidBody : listOfRigidbodies) { Transform3d tmpTransform = new Transform3d(); tmpTransform.setTranslation(rigidBody.xPosition,rigidBody.yPosition,rigidBody.zPosition); tmpTransform.setRotation(new Quat4d(rigidBody.qx, rigidBody.qy, rigidBody.qz, rigidBody.qw)); tfPublisher.publish(tmpTransform, mainNode.getCurrentTime().totalNsecs(), "/mocap_world", "mocap/rigidBody"+rigidBody.getId()); } //System.out.println("Update rate: " + frequencyCalculator.determineCallFrequency() + " Hz"); }
@Override public void updateRigidbodies(ArrayList<MocapRigidBody> listOfRigidbodies) { if(!mainNode.isStarted()) return; for (MocapRigidBody rigidBody : listOfRigidbodies) { RigidBodyTransform tmpTransform = new RigidBodyTransform(); tmpTransform.setTranslation(rigidBody.xPosition,rigidBody.yPosition,rigidBody.zPosition); tmpTransform.setRotation(new Quaternion(rigidBody.qx, rigidBody.qy, rigidBody.qz, rigidBody.qw)); tfPublisher.publish(tmpTransform, mainNode.getCurrentTime().totalNsecs(), "/mocap_world", "mocap/rigidBody"+rigidBody.getId()); } //System.out.println("Update rate: " + frequencyCalculator.determineCallFrequency() + " Hz"); }
@Override public void updateRigidbodies(ArrayList<MocapRigidBody> listOfRigidbodies) { if(!mainNode.isStarted()) return; for (MocapRigidBody rigidBody : listOfRigidbodies) { Transform3d tmpTransform = new Transform3d(); tmpTransform.setTranslation(rigidBody.xPosition,rigidBody.yPosition,rigidBody.zPosition); tmpTransform.setRotation(new Quat4d(rigidBody.qx, rigidBody.qy, rigidBody.qz, rigidBody.qw)); tfPublisher.publish(tmpTransform, mainNode.getCurrentTime().totalNsecs(), "/mocap_world", "mocap/rigidBody"+rigidBody.getId()); } //System.out.println("Update rate: " + frequencyCalculator.determineCallFrequency() + " Hz"); }
public void updateRigidbodies(ArrayList<MocapRigidBody> listOfRigidbodies) { listOfConvertedRigidBodies = new ArrayList<>(); for (MocapRigidBody rb : listOfRigidbodies) { // if (rb.getId() == 1) // { rigidBodyInformationInMocapOrigin = new RigidBodyTransform(new Quaternion(rb.qx, rb.qy, rb.qz, rb.qw), new Vector3D(rb.xPosition, rb.yPosition, rb.zPosition)); mocapRB.update(); mocapRBZUp.update(); FramePose3D rigidBodyPoseInZUp = new FramePose3D(mocapRBZUp, new Point3D(0, 0, 0), new Quaternion()); rigidBodyPoseInZUp.changeFrame(ReferenceFrame.getWorldFrame()); final Vector3D position = new Vector3D(rigidBodyPoseInZUp.getPosition()); final Quaternion rotation = new Quaternion(rigidBodyPoseInZUp.getOrientation()); listOfConvertedRigidBodies.add(new MocapRigidBody(rb.getId(), position, rotation, null, true)); // } } for (MocapRigidbodiesListener listener : listOfMocapRigidBodiesListeners) { listener.updateRigidbodies(listOfConvertedRigidBodies); } } }
rigidBodyPoseInZUp.getOrientation(rotation); listOfConvertedRigidBodies.add(new MocapRigidBody(rb.getId(), position, rotation, null, true));
@Override protected void updateListeners(ArrayList<MocapRigidBody> lisftOfRigidbodies) { ArrayList<MocapRigidBody> convertedListOfMocapRigidBodies = new ArrayList<MocapRigidBody>(); for (MocapRigidBody mocapRigidBody : lisftOfRigidbodies) { mocapRbToMocapOrigin = new RigidBodyTransform(new Quaternion(mocapRigidBody.qx, mocapRigidBody.qy, mocapRigidBody.qz, mocapRigidBody.qw), new Vector3D(mocapRigidBody.xPosition, mocapRigidBody.yPosition, mocapRigidBody.zPosition)); mocapRbFrame.update(); mocapOriginFrame.update(); mocapRbZUpFrame.update(); FramePose3D pose = new FramePose3D(mocapRbZUpFrame, new Point3D(), new Quaternion()); pose.changeFrame(ReferenceFrame.getWorldFrame()); RigidBodyTransform r = new RigidBodyTransform(); pose.get(r); Vector3D position = new Vector3D(); r.getTranslation(position); Quaternion rotation = new Quaternion(); r.getRotation(rotation); convertedListOfMocapRigidBodies.add(new MocapRigidBody(mocapRigidBody.getId(), position, rotation, mocapRigidBody.getListOfAssociatedMarkers(), mocapRigidBody.dataValid)); } updateRigidBodiesListeners(convertedListOfMocapRigidBodies); }
@Override protected void updateListeners(ArrayList<MocapRigidBody> lisftOfRigidbodies) { ArrayList<MocapRigidBody> convertedListOfMocapRigidBodies = new ArrayList<MocapRigidBody>(); for (MocapRigidBody mocapRigidBody : lisftOfRigidbodies) { mocapRbToMocapOrigin = new RigidBodyTransform(new Quat4d(mocapRigidBody.qx, mocapRigidBody.qy, mocapRigidBody.qz, mocapRigidBody.qw), new Vector3d(mocapRigidBody.xPosition, mocapRigidBody.yPosition, mocapRigidBody.zPosition)); mocapRbFrame.update(); mocapOriginFrame.update(); mocapRbZUpFrame.update(); FramePose pose = new FramePose(mocapRbZUpFrame, new Point3d(), new Quat4d()); pose.changeFrame(ReferenceFrame.getWorldFrame()); RigidBodyTransform r = new RigidBodyTransform(); pose.getRigidBodyTransform(r); Vector3d position = new Vector3d(); r.getTranslation(position); Quat4d rotation = new Quat4d(); r.getRotation(rotation); convertedListOfMocapRigidBodies.add(new MocapRigidBody(mocapRigidBody.getId(), position, rotation, mocapRigidBody.getListOfAssociatedMarkers(), mocapRigidBody.dataValid)); } updateRigidBodiesListeners(convertedListOfMocapRigidBodies); }
@Override protected void updateListeners(ArrayList<MocapRigidBody> lisftOfRigidbodies) { ArrayList<MocapRigidBody> convertedListOfMocapRigidBodies = new ArrayList<MocapRigidBody>(); for (MocapRigidBody mocapRigidBody : lisftOfRigidbodies) { mocapRbToMocapOrigin = new RigidBodyTransform(new Quat4d(mocapRigidBody.qx, mocapRigidBody.qy, mocapRigidBody.qz, mocapRigidBody.qw), new Vector3d(mocapRigidBody.xPosition, mocapRigidBody.yPosition, mocapRigidBody.zPosition)); mocapRbFrame.update(); mocapOriginFrame.update(); mocapRbZUpFrame.update(); FramePose pose = new FramePose(mocapRbZUpFrame, new Point3d(), new Quat4d()); pose.changeFrame(ReferenceFrame.getWorldFrame()); RigidBodyTransform r = new RigidBodyTransform(); pose.getRigidBodyTransform(r); Vector3d position = new Vector3d(); r.getTranslation(position); Quat4d rotation = new Quat4d(); r.getRotation(rotation); convertedListOfMocapRigidBodies.add(new MocapRigidBody(mocapRigidBody.getId(), position, rotation, mocapRigidBody.getListOfAssociatedMarkers(), mocapRigidBody.dataValid)); } updateRigidBodiesListeners(convertedListOfMocapRigidBodies); }