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); }
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); }
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; }