public ScsMocapRigidBody(MocapRigidBody mocapRigidBody) { this(mocapRigidBody.getId(), mocapRigidBody.getPosition(), mocapRigidBody.getOrientation(), mocapRigidBody.getListOfAssociatedMarkers(), mocapRigidBody.dataValid); }
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); }
@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); }
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; }
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 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); } } }
public void computePelvisToWorldTransform(MocapRigidBody mocapRigidBody, RigidBodyTransform pelvisToWorldTransformToPack) { RigidBodyTransform pelvisToMocapTransform = new RigidBodyTransform(mocapRigidBody.getOrientation(), mocapRigidBody.getPosition()); pelvisToMocapTransform.multiply(pelvisToMarker2Transform); FramePose3D pelvisPose = new FramePose3D(mocapFrame, pelvisToMocapTransform); pelvisPose.changeFrame(ReferenceFrame.getWorldFrame()); pelvisPose.get(pelvisToWorldTransformToPack); }
rigidBody = new MocapRigidBody(data.rigidBodyId, new Vector3d(data.posX, data.posY, data.posZ), new Quat4d(data.rotX, data.rotY, data.rotZ, data.rotW), listfOfMarkersForThisRB, data.isTracked); rigidBody.toString();
@Override public void updateRigidbodies(ArrayList<MocapRigidBody> listOfRigidbodies) { for (MocapRigidBody rb : listOfRigidbodies) { System.out.println(rb.toString()); } } }
public void update(MocapRigidBody mocapObject) { if(enableMocapUpdates) { mocapObject.packPose(workingRigidBodyTransform); workingRigidBodyTransform.multiply(mocapJigCalibrationTransform); workingRigidBodyTransform.multiply(transformFromMocapHeadCentroidToHeadRoot); mocapHeadPoseInZUp.set(workingRigidBodyTransform); mocapHeadFrame.update(); robotDataReceiver.updateRobotModel(); mocapHeadFrame.getTransformToDesiredFrame(transformFromMocapHeadToRobotHead , robotHeadFrame); mocapOffsetFrame.update(); } }
@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); }
listOfModels.add("" + rb.getId());
@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); } } }
rigidBodyPoseInZUp.getOrientation(rotation); listOfConvertedRigidBodies.add(new MocapRigidBody(rb.getId(), position, rotation, null, true));
public void initialize(ReferenceFrame pelvisFrame, MocapRigidBody markerRigidBody) { RigidBodyTransform marker2ToMocapTransform = new RigidBodyTransform(markerRigidBody.getOrientation(), markerRigidBody.getPosition()); RigidBodyTransform worldToPelvisTransform = pelvisFrame.getTransformToWorldFrame(); worldToPelvisTransform.invert(); RigidBodyTransform worldToMocapTransform = new RigidBodyTransform(); worldToMocapTransform.multiply(marker2ToMocapTransform); worldToMocapTransform.multiply(pelvisToMarker2Transform); worldToMocapTransform.multiply(worldToPelvisTransform); mocapFrame = ReferenceFrame.constructFrameWithUnchangingTransformFromParent("mocapFrame", ReferenceFrame.getWorldFrame(), worldToMocapTransform); initialized = true; }
rigidBody = new MocapRigidBody(data.rigidBodyId, new Vector3D(data.posX, data.posY, data.posZ), new Quaternion(data.rotX, data.rotY, data.rotZ, data.rotW), listfOfMarkersForThisRB, data.isTracked); rigidBody.toString();
@Override public void updateRigidbodies(ArrayList<MocapRigidBody> listOfRigidbodies) { for (MocapRigidBody rb : listOfRigidbodies) { System.out.println(rb.toString()); } } }
public void update(MocapRigidBody mocapObject) { if(enableMocapUpdates) { mocapObject.packPose(workingRigidBodyTransform); workingRigidBodyTransform.multiply(mocapJigCalibrationTransform); workingRigidBodyTransform.multiply(transformFromMocapHeadCentroidToHeadRoot); mocapHeadPoseInZUp.set(workingRigidBodyTransform); mocapHeadFrame.update(); robotDataReceiver.updateRobotModel(); mocapHeadFrame.getTransformToDesiredFrame(transformFromMocapHeadToRobotHead , robotHeadFrame); mocapOffsetFrame.update(); } }
public ScsMocapRigidBody(MocapRigidBody mocapRigidBody) { this(mocapRigidBody.getId(), mocapRigidBody.getPosition(), mocapRigidBody.getOrientation(), mocapRigidBody.getListOfAssociatedMarkers(), mocapRigidBody.dataValid); }