public HandstepPacket(RobotSide robotSide, Point3d location, Quat4d orientation, Vector3d surfaceNormal, double swingTrajectoryTime) { this.robotSide = robotSide; this.location = location; this.orientation = orientation; this.surfaceNormal = surfaceNormal; this.swingTrajectoryTime = swingTrajectoryTime; RotationTools.checkQuaternionNormalized(this.orientation); }
public void setOrientation(Quat4d orientation) { this.orientation.set(orientation); RotationTools.checkQuaternionNormalized(this.orientation); }
public AdjustFootstepMessage(AdjustFootstepMessage footstepData) { uniqueId = VALID_MESSAGE_DEFAULT_ID; this.origin = footstepData.origin; this.robotSide = footstepData.robotSide; this.location = new Point3d(footstepData.location); this.orientation = new Quat4d(footstepData.orientation); RotationTools.checkQuaternionNormalized(this.orientation); if (footstepData.predictedContactPoints == null || footstepData.predictedContactPoints.isEmpty()) { this.predictedContactPoints = null; } else { this.predictedContactPoints = new ArrayList<>(); for (Point2d contactPoint : footstepData.predictedContactPoints) { this.predictedContactPoints.add(new Point2d(contactPoint)); } } }
public HandstepPacket(HandstepPacket handstepPacket) { this.robotSide = handstepPacket.robotSide; this.location = new Point3d(handstepPacket.location); this.orientation = new Quat4d(handstepPacket.orientation); this.surfaceNormal = new Vector3d(handstepPacket.surfaceNormal); RotationTools.checkQuaternionNormalized(this.orientation); }
this.location = new Point3d(footstepData.location); this.orientation = new Quat4d(footstepData.orientation); RotationTools.checkQuaternionNormalized(this.orientation); if (footstepData.predictedContactPoints == null || footstepData.predictedContactPoints.isEmpty())
@Override public void setConfiguration(DenseMatrix64F matrix, int rowStart) { int index = rowStart; MatrixTools.extractQuat4dFromEJMLVector(jointRotation, matrix, rowStart); RotationTools.checkQuaternionNormalized(jointRotation); index += RotationTools.QUATERNION_SIZE; jointTranslation.setX(matrix.get(index++, 0)); jointTranslation.setY(matrix.get(index++, 0)); jointTranslation.setZ(matrix.get(index++, 0)); this.afterJointFrame.setRotation(jointRotation); this.afterJointFrame.setTranslation(jointTranslation); }
public void setRotation(Matrix3d rotationMatrix, int imuIndex) { Quat4d q = new Quat4d(); RotationTools.convertMatrixToQuaternion(rotationMatrix, q); RotationTools.checkQuaternionNormalized(q); // DON'T USE THIS: the method in Quat4d is flawed and doesn't work for some rotation matrices! // q.set(rotationMatrix); p_qs.set(q.getW()); p_qx.set(q.getX()); p_qy.set(q.getY()); p_qz.set(q.getZ()); p_yaw.set(RotationTools.computeYaw(rotationMatrix)); p_pitch.set(RotationTools.computePitch(rotationMatrix)); p_roll.set(RotationTools.computeRoll(rotationMatrix)); }